mawei
/
openpilot1
mirror of https://github.com/huifan0114/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Temporary GM longitudinal fixes

This commit is contained in:
FrogAi 2024-07-31 13:03:33 -07:00
parent 96e847e051
commit 8699f29666
9 changed files with 174 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cereal/car.capnp
View File

@ -582,8 +582,8 @@ struct CarParams {
kiBP @2 :List(Float32);
kiV @3 :List(Float32);
kf @6 :Float32;
deadzoneBPDEPRECATED @4 :List(Float32);
deadzoneVDEPRECATED @5 :List(Float32);
deadzoneBP @4 :List(Float32);
deadzoneV @5 :List(Float32);
}
struct LateralINDITuning {

2
cereal/log.capnp
View File

@ -704,6 +704,7 @@ struct ControlsState @0x97ff69c53601abf1 {
personality @66 :LongitudinalPersonality;
longControlState @30 :Car.CarControl.Actuators.LongControlState;
vPid @2 :Float32;
vTargetLead @3 :Float32;
vCruise @22 :Float32; # actual set speed
vCruiseCluster @63 :Float32; # set speed to display in the UI
@ -873,7 +874,6 @@ struct ControlsState @0x97ff69c53601abf1 {
canMonoTimesDEPRECATED @21 :List(UInt64);
desiredCurvatureRateDEPRECATED @62 :Float32;
canErrorCounterDEPRECATED @57 :UInt32;
vPidDEPRECATED @2 :Float32;
}
# All SI units and in device frame

1
common/params.cc
View File

@ -389,6 +389,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"MTSCEnabled", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
{"NavigationModels", PERSISTENT},
{"NextMapSpeedLimit", PERSISTENT},
{"NewLongAPIGM", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES},
{"NNFF", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
{"NNFFLite", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
{"NNFFModelName", PERSISTENT},

62
selfdrive/car/gm/interface.py
View File

@ -98,6 +98,8 @@ class CarInterface(CarInterfaceBase):
@staticmethod
def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs, params):
use_new_api = params.get_bool("NewLongAPIGM")
ret.carName = "gm"
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)]
ret.autoResumeSng = False
@ -111,7 +113,14 @@ class CarInterface(CarInterfaceBase):
else:
ret.transmissionType = TransmissionType.automatic
ret.longitudinalTuning.kiBP = [5., 35.]
if use_new_api:
ret.longitudinalTuning.kiBP = [5., 35.]
else:
ret.longitudinalTuning.deadzoneBP = [0.]
ret.longitudinalTuning.deadzoneV = [0.15]
ret.longitudinalTuning.kpBP = [5., 35.]
ret.longitudinalTuning.kiBP = [0.]
if candidate in CAMERA_ACC_CAR:
ret.experimentalLongitudinalAvailable = candidate not in CC_ONLY_CAR
@ -123,9 +132,17 @@ class CarInterface(CarInterfaceBase):
ret.minSteerSpeed = 10 * CV.KPH_TO_MS
# Tuning for experimental long
ret.longitudinalTuning.kiV = [2.0, 1.5]
ret.vEgoStopping = 0.1
ret.vEgoStarting = 0.1
if use_new_api:
ret.longitudinalTuning.kiV = [2.0, 1.5]
ret.vEgoStopping = 0.1
ret.vEgoStarting = 0.1
else:
ret.longitudinalTuning.kpV = [2.0, 1.5]
ret.longitudinalTuning.kiV = [0.72]
ret.stoppingDecelRate = 2.0 # reach brake quickly after enabling
ret.vEgoStopping = 0.25
ret.vEgoStarting = 0.25
if experimental_long:
ret.pcmCruise = False
@ -133,6 +150,8 @@ class CarInterface(CarInterfaceBase):
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM_LONG
elif candidate in SDGM_CAR:
if use_new_api:
ret.longitudinalTuning.kiV = [0., 0.] # TODO: tuning
ret.experimentalLongitudinalAvailable = False
ret.networkLocation = NetworkLocation.fwdCamera
ret.pcmCruise = True
@ -151,7 +170,11 @@ class CarInterface(CarInterfaceBase):
ret.minSteerSpeed = 7 * CV.MPH_TO_MS
# Tuning
ret.longitudinalTuning.kiV = [2.4, 1.5]
if use_new_api:
ret.longitudinalTuning.kiV = [2.4, 1.5]
else:
ret.longitudinalTuning.kpV = [2.4, 1.5]
ret.longitudinalTuning.kiV = [0.36]
if ret.enableGasInterceptor:
# Need to set ASCM long limits when using pedal interceptor, instead of camera ACC long limits
@ -271,8 +294,14 @@ class CarInterface(CarInterfaceBase):
ret.flags |= GMFlags.PEDAL_LONG.value
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_PEDAL_LONG
# Note: Low speed, stop and go not tested. Should be fairly smooth on highway
ret.longitudinalTuning.kiBP = [0.0, 5., 35.]
ret.longitudinalTuning.kiV = [0.0, 0.35, 0.5]
if use_new_api:
ret.longitudinalTuning.kiBP = [0.0, 5., 35.]
ret.longitudinalTuning.kiV = [0.0, 0.35, 0.5]
else:
ret.longitudinalTuning.kpBP = [5., 35.]
ret.longitudinalTuning.kpV = [0.35, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.0]
ret.longitudinalTuning.kiV = [0.1, 0.1]
ret.longitudinalTuning.kf = 0.15
ret.stoppingDecelRate = 0.8
else: # Pedal used for SNG, ACC for longitudinal control otherwise
@ -290,14 +319,21 @@ class CarInterface(CarInterfaceBase):
ret.openpilotLongitudinalControl = not disable_openpilot_long
ret.pcmCruise = False
if use_new_api:
ret.longitudinalTuning.kiBP = [10.7, 10.8, 28.]
ret.longitudinalTuning.kiV = [0., 20., 20.] # set lower end to 0 since we can't drive below that speed
else:
ret.longitudinalTuning.deadzoneBP = [0.]
ret.longitudinalTuning.deadzoneV = [0.56] # == 2 km/h/s, 1.25 mph/s
ret.longitudinalActuatorDelay = 1. # TODO: measure this
ret.longitudinalTuning.kpBP = [10.7, 10.8, 28.] # 10.7 m/s == 24 mph
ret.longitudinalTuning.kpV = [0., 20., 20.] # set lower end to 0 since we can't drive below that speed
ret.longitudinalTuning.kiBP = [0.]
ret.longitudinalTuning.kiV = [0.1]
ret.stoppingDecelRate = 11.18 # == 25 mph/s (.04 rate)
ret.longitudinalActuatorDelayLowerBound = 1. # TODO: measure this
ret.longitudinalActuatorDelayUpperBound = 2.
ret.longitudinalTuning.kiBP = [10.7, 10.8, 28.]
ret.longitudinalTuning.kiV = [0., 20., 20.] # set lower end to 0 since we can't drive below that speed
if candidate in CC_ONLY_CAR:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_NO_ACC

13
selfdrive/controls/controlsd.py
View File

@ -209,6 +209,7 @@ class Controls:
self.speed_limit_changed = False
self.stopped_for_light = False
self.update_toggles = False
self.use_old_long = self.CP.carName == "gm" and not self.params.get_bool("NewLongAPIGM")
self.display_timer = 0
self.drive_distance = 0
@ -628,12 +629,19 @@ class Controls:
if not CC.latActive:
self.LaC.reset()
if not CC.longActive:
self.LoC.reset()
if self.use_old_long:
self.LoC.reset_old_long(v_pid=CS.vEgo)
else:
self.LoC.reset()
if not self.joystick_mode:
# accel PID loop
pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_helper.v_cruise_kph * CV.KPH_TO_MS, self.frogpilot_toggles)
actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits)
if self.use_old_long:
t_since_plan = (self.sm.frame - self.sm.recv_frame['longitudinalPlan']) * DT_CTRL
actuators.accel = self.LoC.update_old_long(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan)
else:
actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits)
if len(long_plan.speeds):
actuators.speed = long_plan.speeds[-1]
@ -838,6 +846,7 @@ class Controls:
controlsState.state = self.state
controlsState.engageable = not self.events.contains(ET.NO_ENTRY)
controlsState.longControlState = self.LoC.long_control_state
controlsState.vPid = float(self.LoC.v_pid)
controlsState.vCruise = float(self.v_cruise_helper.v_cruise_kph)
controlsState.vCruiseCluster = float(self.v_cruise_helper.v_cruise_cluster_kph)
controlsState.upAccelCmd = float(self.LoC.pid.p)

109
selfdrive/controls/lib/longcontrol.py
View File

@ -1,7 +1,7 @@
from cereal import car
from openpilot.common.numpy_fast import clip
from openpilot.common.numpy_fast import clip, interp
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, apply_deadzone
from openpilot.selfdrive.controls.lib.pid import PIDController
from openpilot.selfdrive.modeld.constants import ModelConstants
@ -46,6 +46,45 @@ def long_control_state_trans(CP, active, long_control_state, v_ego,
long_control_state = LongCtrlState.pid
return long_control_state
def long_control_state_trans_old_long(CP, active, long_control_state, v_ego, v_target,
v_target_1sec, brake_pressed, cruise_standstill):
accelerating = v_target_1sec > v_target
planned_stop = (v_target < CP.vEgoStopping and
v_target_1sec < CP.vEgoStopping and
not accelerating)
stay_stopped = (v_ego < CP.vEgoStopping and
(brake_pressed or cruise_standstill))
stopping_condition = planned_stop or stay_stopped
starting_condition = (v_target_1sec > CP.vEgoStarting and
accelerating and
not cruise_standstill and
not brake_pressed)
started_condition = v_ego > CP.vEgoStarting
if not active:
long_control_state = LongCtrlState.off
else:
if long_control_state in (LongCtrlState.off, LongCtrlState.pid):
long_control_state = LongCtrlState.pid
if stopping_condition:
long_control_state = LongCtrlState.stopping
elif long_control_state == LongCtrlState.stopping:
if starting_condition and CP.startingState:
long_control_state = LongCtrlState.starting
elif starting_condition:
long_control_state = LongCtrlState.pid
elif long_control_state == LongCtrlState.starting:
if stopping_condition:
long_control_state = LongCtrlState.stopping
elif started_condition:
long_control_state = LongCtrlState.pid
return long_control_state
class LongControl:
def __init__(self, CP):
self.CP = CP
@ -53,6 +92,7 @@ class LongControl:
self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
k_f=CP.longitudinalTuning.kf, rate=1 / DT_CTRL)
self.v_pid = 0.0
self.last_output_accel = 0.0
def reset(self):
@ -88,3 +128,68 @@ class LongControl:
self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
return self.last_output_accel
def reset_old_long(self, v_pid):
"""Reset PID controller and change setpoint"""
self.pid.reset()
self.v_pid = v_pid
def update_old_long(self, active, CS, long_plan, accel_limits, t_since_plan):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Interp control trajectory
speeds = long_plan.speeds
if len(speeds) == CONTROL_N:
v_target_now = interp(t_since_plan, CONTROL_N_T_IDX, speeds)
a_target_now = interp(t_since_plan, CONTROL_N_T_IDX, long_plan.accels)
v_target = interp(self.CP.longitudinalActuatorDelay + t_since_plan, CONTROL_N_T_IDX, speeds)
a_target = 2 * (v_target - v_target_now) / self.CP.longitudinalActuatorDelay - a_target_now
v_target_1sec = interp(self.CP.longitudinalActuatorDelay + t_since_plan + 1.0, CONTROL_N_T_IDX, speeds)
else:
v_target = 0.0
v_target_now = 0.0
v_target_1sec = 0.0
a_target = 0.0
self.pid.neg_limit = accel_limits[0]
self.pid.pos_limit = accel_limits[1]
output_accel = self.last_output_accel
self.long_control_state = long_control_state_trans_old_long(self.CP, active, self.long_control_state, CS.vEgo,
v_target, v_target_1sec, CS.brakePressed,
CS.cruiseState.standstill)
if self.long_control_state == LongCtrlState.off:
self.reset_old_long(CS.vEgo)
output_accel = 0.
elif self.long_control_state == LongCtrlState.stopping:
if output_accel > self.CP.stopAccel:
output_accel = min(output_accel, 0.0)
output_accel -= self.CP.stoppingDecelRate * DT_CTRL
self.reset_old_long(CS.vEgo)
elif self.long_control_state == LongCtrlState.starting:
output_accel = self.CP.startAccel
self.reset_old_long(CS.vEgo)
elif self.long_control_state == LongCtrlState.pid:
self.v_pid = v_target_now
# Toyota starts braking more when it thinks you want to stop
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
# TODO too complex, needs to be simplified and tested on toyotas
prevent_overshoot = not self.CP.stoppingControl and CS.vEgo < 1.5 and v_target_1sec < 0.7 and v_target_1sec < self.v_pid
deadzone = interp(CS.vEgo, self.CP.longitudinalTuning.deadzoneBP, self.CP.longitudinalTuning.deadzoneV)
freeze_integrator = prevent_overshoot
error = self.v_pid - CS.vEgo
error_deadzone = apply_deadzone(error, deadzone)
output_accel = self.pid.update(error_deadzone, speed=CS.vEgo,
feedforward=a_target,
freeze_integrator=freeze_integrator)
self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
return self.last_output_accel

1
selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc
View File

@ -140,6 +140,7 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil
std::vector<std::tuple<QString, QString, QString, QString>> vehicleToggles {
{"LongPitch", tr("Long Pitch Compensation"), tr("Smoothen out the gas and pedal controls."), ""},
{"VoltSNG", tr("2017 Volt SNG"), tr("Enable the 'Stop and Go' hack for 2017 Chevy Volts."), ""},
{"NewLongAPIGM", tr("Use comma's New Longitudinal API"), tr("Use comma's new longitudinal controls that have shown great improvement with acceleration and braking, but has a few issues on some GM vehicles."), ""},
{"CrosstrekTorque", tr("Subaru Crosstrek Torque Increase"), tr("Increases the maximum allowed torque for the Subaru Crosstrek."), ""},

2
selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h
View File

@ -31,7 +31,7 @@ private:
QMap<QString, QString> carModels;
std::set<QString> gmKeys = {"LongPitch", "VoltSNG"};
std::set<QString> gmKeys = {"LongPitch", "NewLongAPIGM", "VoltSNG"};
std::set<QString> subaruKeys = {"CrosstrekTorque"};
std::set<QString> toyotaKeys = {"ClusterOffset", "SNGHack", "ToyotaDoors", "ToyotaTune"};

1
system/manager/manager.py
View File

@ -213,6 +213,7 @@ def manager_init() -> None:
("MTSCCurvatureCheck", "0"),
("MTSCEnabled", "1"),
("NavigationModels", ""),
("NewLongAPIGM", "1"),
("NNFF", "1"),
("NNFFLite", "1"),
("NoLogging", "0"),