long control: new API (#32706)
* Simplify long control * Seperate * Rename * Try new api for toyota * rm v_pid everywhere * No speed in reset * 0 is better default * unassigned variable * Update other cars * Update gm * SIMPLIFY * simplify more * fix API boundry * Fix stopping bug * Small fixes * Update ref
This commit is contained in:
Harald Schäfer
GitHub
parent
38529c5057
commit
bc303df6a0
|
|
@ -541,8 +541,8 @@ struct CarParams {
|
|||
kiBP @2 :List(Float32);
|
||||
kiV @3 :List(Float32);
|
||||
kf @6 :Float32;
|
||||
deadzoneBP @4 :List(Float32);
|
||||
deadzoneV @5 :List(Float32);
|
||||
deadzoneBPDEPRECATED @4 :List(Float32);
|
||||
deadzoneVDEPRECATED @5 :List(Float32);
|
||||
}
|
||||
|
||||
struct LateralINDITuning {
|
||||
|
|
|
|||
|
|
@ -698,7 +698,6 @@ 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
|
||||
|
|
@ -866,6 +865,7 @@ struct ControlsState @0x97ff69c53601abf1 {
|
|||
canMonoTimesDEPRECATED @21 :List(UInt64);
|
||||
desiredCurvatureRateDEPRECATED @62 :Float32;
|
||||
canErrorCounterDEPRECATED @57 :UInt32;
|
||||
vPidDEPRECATED @2 :Float32;
|
||||
}
|
||||
|
||||
# All SI units and in device frame
|
||||
|
|
@ -1060,6 +1060,11 @@ struct LongitudinalPlan @0xe00b5b3eba12876c {
|
|||
accels @32 :List(Float32);
|
||||
speeds @33 :List(Float32);
|
||||
jerks @34 :List(Float32);
|
||||
aTarget @18 :Float32;
|
||||
shouldStop @37: Bool;
|
||||
allowThrottle @38: Bool;
|
||||
allowBrake @39: Bool;
|
||||
|
||||
|
||||
solverExecutionTime @35 :Float32;
|
||||
|
||||
|
|
@ -1076,7 +1081,6 @@ struct LongitudinalPlan @0xe00b5b3eba12876c {
|
|||
aCruiseDEPRECATED @17 :Float32;
|
||||
vTargetDEPRECATED @3 :Float32;
|
||||
vTargetFutureDEPRECATED @14 :Float32;
|
||||
aTargetDEPRECATED @18 :Float32;
|
||||
vStartDEPRECATED @26 :Float32;
|
||||
aStartDEPRECATED @27 :Float32;
|
||||
vMaxDEPRECATED @20 :Float32;
|
||||
|
|
|
|||
|
|
@ -22,10 +22,6 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.steerActuatorDelay = 0.2
|
||||
ret.steerLimitTimer = 1.0
|
||||
|
||||
ret.longitudinalTuning.kpBP = [0.]
|
||||
ret.longitudinalTuning.kpV = [0.5]
|
||||
ret.longitudinalTuning.kiV = [0.]
|
||||
|
||||
CAN = CanBus(fingerprint=fingerprint)
|
||||
cfgs = [get_safety_config(car.CarParams.SafetyModel.ford)]
|
||||
if CAN.main >= 4:
|
||||
|
|
|
|||
|
|
@ -94,11 +94,7 @@ class CarInterface(CarInterfaceBase):
|
|||
else:
|
||||
ret.transmissionType = TransmissionType.automatic
|
||||
|
||||
ret.longitudinalTuning.deadzoneBP = [0.]
|
||||
ret.longitudinalTuning.deadzoneV = [0.15]
|
||||
|
||||
ret.longitudinalTuning.kpBP = [5., 35.]
|
||||
ret.longitudinalTuning.kiBP = [0.]
|
||||
ret.longitudinalTuning.kiBP = [5., 35.]
|
||||
|
||||
if candidate in CAMERA_ACC_CAR:
|
||||
ret.experimentalLongitudinalAvailable = True
|
||||
|
|
@ -110,8 +106,7 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.minSteerSpeed = 10 * CV.KPH_TO_MS
|
||||
|
||||
# Tuning for experimental long
|
||||
ret.longitudinalTuning.kpV = [2.0, 1.5]
|
||||
ret.longitudinalTuning.kiV = [0.72]
|
||||
ret.longitudinalTuning.kiV = [2.0, 1.5]
|
||||
ret.stoppingDecelRate = 2.0 # reach brake quickly after enabling
|
||||
ret.vEgoStopping = 0.25
|
||||
ret.vEgoStarting = 0.25
|
||||
|
|
@ -131,8 +126,7 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.minSteerSpeed = 7 * CV.MPH_TO_MS
|
||||
|
||||
# Tuning
|
||||
ret.longitudinalTuning.kpV = [2.4, 1.5]
|
||||
ret.longitudinalTuning.kiV = [0.36]
|
||||
ret.longitudinalTuning.kiV = [2.4, 1.5]
|
||||
|
||||
# These cars have been put into dashcam only due to both a lack of users and test coverage.
|
||||
# These cars likely still work fine. Once a user confirms each car works and a test route is
|
||||
|
|
|
|||
|
|
@ -72,17 +72,13 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.lateralTuning.pid.kf = 0.00006 # conservative feed-forward
|
||||
|
||||
if candidate in HONDA_BOSCH:
|
||||
ret.longitudinalTuning.kpV = [0.25]
|
||||
ret.longitudinalTuning.kiV = [0.05]
|
||||
ret.longitudinalActuatorDelay = 0.5 # s
|
||||
if candidate in HONDA_BOSCH_RADARLESS:
|
||||
ret.stopAccel = CarControllerParams.BOSCH_ACCEL_MIN # stock uses -4.0 m/s^2 once stopped but limited by safety model
|
||||
else:
|
||||
# default longitudinal tuning for all hondas
|
||||
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
||||
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
|
||||
ret.longitudinalTuning.kiBP = [0., 35.]
|
||||
ret.longitudinalTuning.kiV = [0.18, 0.12]
|
||||
ret.longitudinalTuning.kiBP = [0., 5., 35.]
|
||||
ret.longitudinalTuning.kiV = [1.2, 0.8, 0.5]
|
||||
|
||||
eps_modified = False
|
||||
for fw in car_fw:
|
||||
|
|
|
|||
|
|
@ -80,12 +80,8 @@ class CarInterface(CarInterfaceBase):
|
|||
|
||||
# *** longitudinal control ***
|
||||
if candidate in CANFD_CAR:
|
||||
ret.longitudinalTuning.kpV = [0.1]
|
||||
ret.longitudinalTuning.kiV = [0.0]
|
||||
ret.experimentalLongitudinalAvailable = candidate not in (CANFD_UNSUPPORTED_LONGITUDINAL_CAR | CANFD_RADAR_SCC_CAR)
|
||||
else:
|
||||
ret.longitudinalTuning.kpV = [0.5]
|
||||
ret.longitudinalTuning.kiV = [0.0]
|
||||
ret.experimentalLongitudinalAvailable = candidate not in (UNSUPPORTED_LONGITUDINAL_CAR | CAMERA_SCC_CAR)
|
||||
ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable
|
||||
ret.pcmCruise = not ret.openpilotLongitudinalControl
|
||||
|
|
|
|||
|
|
@ -202,13 +202,11 @@ class CarInterfaceBase(ABC):
|
|||
ret.vEgoStopping = 0.5
|
||||
ret.vEgoStarting = 0.5
|
||||
ret.stoppingControl = True
|
||||
ret.longitudinalTuning.deadzoneBP = [0.]
|
||||
ret.longitudinalTuning.deadzoneV = [0.]
|
||||
ret.longitudinalTuning.kf = 1.
|
||||
ret.longitudinalTuning.kpBP = [0.]
|
||||
ret.longitudinalTuning.kpV = [1.]
|
||||
ret.longitudinalTuning.kpV = [0.]
|
||||
ret.longitudinalTuning.kiBP = [0.]
|
||||
ret.longitudinalTuning.kiV = [1.]
|
||||
ret.longitudinalTuning.kiV = [0.]
|
||||
# TODO estimate car specific lag, use .15s for now
|
||||
ret.longitudinalActuatorDelay = 0.15
|
||||
ret.steerLimitTimer = 1.0
|
||||
|
|
|
|||
|
|
@ -91,11 +91,6 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.flags |= SubaruFlags.DISABLE_EYESIGHT.value
|
||||
|
||||
if ret.openpilotLongitudinalControl:
|
||||
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
||||
ret.longitudinalTuning.kpV = [0.8, 1.0, 1.5]
|
||||
ret.longitudinalTuning.kiBP = [0., 35.]
|
||||
ret.longitudinalTuning.kiV = [0.54, 0.36]
|
||||
|
||||
ret.stoppingControl = True
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_LONG
|
||||
|
||||
|
|
|
|||
|
|
@ -18,11 +18,6 @@ class CarInterface(CarInterfaceBase):
|
|||
|
||||
ret.steerControlType = car.CarParams.SteerControlType.angle
|
||||
|
||||
# Set kP and kI to 0 over the whole speed range to have the planner accel as actuator command
|
||||
ret.longitudinalTuning.kpBP = [0]
|
||||
ret.longitudinalTuning.kpV = [0]
|
||||
ret.longitudinalTuning.kiBP = [0]
|
||||
ret.longitudinalTuning.kiV = [0]
|
||||
ret.longitudinalActuatorDelay = 0.5 # s
|
||||
ret.radarTimeStep = (1.0 / 8) # 8Hz
|
||||
|
||||
|
|
|
|||
|
|
@ -75,7 +75,6 @@ class TestCarInterfaces:
|
|||
# Longitudinal sanity checks
|
||||
assert len(car_params.longitudinalTuning.kpV) == len(car_params.longitudinalTuning.kpBP)
|
||||
assert len(car_params.longitudinalTuning.kiV) == len(car_params.longitudinalTuning.kiBP)
|
||||
assert len(car_params.longitudinalTuning.deadzoneV) == len(car_params.longitudinalTuning.deadzoneBP)
|
||||
|
||||
# Lateral sanity checks
|
||||
if car_params.steerControlType != car.CarParams.SteerControlType.angle:
|
||||
|
|
|
|||
|
|
@ -142,22 +142,15 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.minEnableSpeed = -1. if stop_and_go else MIN_ACC_SPEED
|
||||
|
||||
tune = ret.longitudinalTuning
|
||||
tune.deadzoneBP = [0., 9.]
|
||||
tune.deadzoneV = [.0, .15]
|
||||
if candidate in TSS2_CAR:
|
||||
tune.kpBP = [0., 5., 20.]
|
||||
tune.kpV = [1.3, 1.0, 0.7]
|
||||
tune.kiBP = [0., 5., 12., 20., 27.]
|
||||
tune.kiV = [.35, .23, .20, .17, .1]
|
||||
if candidate in TSS2_CAR:
|
||||
ret.vEgoStopping = 0.25
|
||||
ret.vEgoStarting = 0.25
|
||||
ret.stoppingDecelRate = 0.3 # reach stopping target smoothly
|
||||
tune.kpV = [0.0]
|
||||
tune.kiV = [0.5]
|
||||
ret.vEgoStopping = 0.25
|
||||
ret.vEgoStarting = 0.25
|
||||
ret.stoppingDecelRate = 0.3 # reach stopping target smoothly
|
||||
else:
|
||||
tune.kpBP = [0., 5., 35.]
|
||||
tune.kiBP = [0., 35.]
|
||||
tune.kpV = [3.6, 2.4, 1.5]
|
||||
tune.kiV = [0.54, 0.36]
|
||||
tune.kiBP = [0., 5., 35.]
|
||||
tune.kiV = [3.6, 2.4, 1.5]
|
||||
|
||||
return ret
|
||||
|
||||
|
|
|
|||
|
|
@ -96,8 +96,6 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.stopAccel = -0.55
|
||||
ret.vEgoStarting = 0.1
|
||||
ret.vEgoStopping = 0.5
|
||||
ret.longitudinalTuning.kpV = [0.1]
|
||||
ret.longitudinalTuning.kiV = [0.0]
|
||||
ret.autoResumeSng = ret.minEnableSpeed == -1
|
||||
|
||||
return ret
|
||||
|
|
|
|||
|
|
@ -563,13 +563,12 @@ class Controls:
|
|||
if not CC.latActive:
|
||||
self.LaC.reset()
|
||||
if not CC.longActive:
|
||||
self.LoC.reset(v_pid=CS.vEgo)
|
||||
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)
|
||||
t_since_plan = (self.sm.frame - self.sm.recv_frame['longitudinalPlan']) * DT_CTRL
|
||||
actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan)
|
||||
actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits)
|
||||
|
||||
# Steering PID loop and lateral MPC
|
||||
self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature)
|
||||
|
|
@ -752,7 +751,6 @@ 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)
|
||||
|
|
|
|||
|
|
@ -141,16 +141,6 @@ class VCruiseHelper:
|
|||
self.v_cruise_cluster_kph = self.v_cruise_kph
|
||||
|
||||
|
||||
def apply_deadzone(error, deadzone):
|
||||
if error > deadzone:
|
||||
error -= deadzone
|
||||
elif error < - deadzone:
|
||||
error += deadzone
|
||||
else:
|
||||
error = 0.
|
||||
return error
|
||||
|
||||
|
||||
def apply_center_deadzone(error, deadzone):
|
||||
if (error > - deadzone) and (error < deadzone):
|
||||
error = 0.
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
from cereal import car
|
||||
from openpilot.common.numpy_fast import clip, interp
|
||||
from openpilot.common.numpy_fast import clip
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, apply_deadzone
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
|
||||
from openpilot.selfdrive.controls.lib.pid import PIDController
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
|
||||
|
|
@ -10,18 +10,10 @@ CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
|
|||
LongCtrlState = car.CarControl.Actuators.LongControlState
|
||||
|
||||
|
||||
def long_control_state_trans(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
|
||||
def long_control_state_trans(CP, active, long_control_state, v_ego,
|
||||
should_stop, brake_pressed, cruise_standstill):
|
||||
stopping_condition = should_stop
|
||||
starting_condition = (not should_stop and
|
||||
not cruise_standstill and
|
||||
not brake_pressed)
|
||||
started_condition = v_ego > CP.vEgoStarting
|
||||
|
|
@ -34,7 +26,6 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, v_target,
|
|||
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
|
||||
|
|
@ -49,78 +40,45 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, v_target,
|
|||
|
||||
return long_control_state
|
||||
|
||||
|
||||
class LongControl:
|
||||
def __init__(self, CP):
|
||||
self.CP = CP
|
||||
self.long_control_state = LongCtrlState.off # initialized to off
|
||||
self.long_control_state = LongCtrlState.off
|
||||
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, v_pid):
|
||||
"""Reset PID controller and change setpoint"""
|
||||
def reset(self):
|
||||
self.pid.reset()
|
||||
self.v_pid = v_pid
|
||||
|
||||
def update(self, active, CS, long_plan, accel_limits, t_since_plan):
|
||||
def update(self, active, CS, a_target, should_stop, accel_limits):
|
||||
"""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(self.CP, active, self.long_control_state, CS.vEgo,
|
||||
v_target, v_target_1sec, CS.brakePressed,
|
||||
should_stop, CS.brakePressed,
|
||||
CS.cruiseState.standstill)
|
||||
|
||||
if self.long_control_state == LongCtrlState.off:
|
||||
self.reset(CS.vEgo)
|
||||
self.reset()
|
||||
output_accel = 0.
|
||||
|
||||
elif self.long_control_state == LongCtrlState.stopping:
|
||||
output_accel = self.last_output_accel
|
||||
if output_accel > self.CP.stopAccel:
|
||||
output_accel = min(output_accel, 0.0)
|
||||
output_accel -= self.CP.stoppingDecelRate * DT_CTRL
|
||||
self.reset(CS.vEgo)
|
||||
self.reset()
|
||||
|
||||
elif self.long_control_state == LongCtrlState.starting:
|
||||
output_accel = self.CP.startAccel
|
||||
self.reset(CS.vEgo)
|
||||
self.reset()
|
||||
|
||||
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)
|
||||
else: # LongCtrlState.pid
|
||||
error = a_target - CS.aEgo
|
||||
output_accel = self.pid.update(error, speed=CS.vEgo,
|
||||
feedforward=a_target)
|
||||
|
||||
self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
|
||||
|
||||
return self.last_output_accel
|
||||
|
|
|
|||
|
|
@ -19,6 +19,7 @@ LON_MPC_STEP = 0.2 # first step is 0.2s
|
|||
A_CRUISE_MIN = -1.2
|
||||
A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
|
||||
A_CRUISE_MAX_BP = [0., 10.0, 25., 40.]
|
||||
CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
|
||||
|
||||
# Lookup table for turns
|
||||
_A_TOTAL_MAX_V = [1.7, 3.2]
|
||||
|
|
@ -34,7 +35,6 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
|
|||
This function returns a limited long acceleration allowed, depending on the existing lateral acceleration
|
||||
this should avoid accelerating when losing the target in turns
|
||||
"""
|
||||
|
||||
# FIXME: This function to calculate lateral accel is incorrect and should use the VehicleModel
|
||||
# The lookup table for turns should also be updated if we do this
|
||||
a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
|
||||
|
|
@ -44,6 +44,26 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
|
|||
return [a_target[0], min(a_target[1], a_x_allowed)]
|
||||
|
||||
|
||||
def get_accel_from_plan(CP, long_plan):
|
||||
speeds = long_plan.speeds
|
||||
if len(speeds) == CONTROL_N:
|
||||
v_target_now = interp(DT_MDL, CONTROL_N_T_IDX, speeds)
|
||||
a_target_now = interp(DT_MDL, CONTROL_N_T_IDX, long_plan.accels)
|
||||
|
||||
v_target = interp(CP.longitudinalActuatorDelay + DT_MDL, CONTROL_N_T_IDX, speeds)
|
||||
a_target = 2 * (v_target - v_target_now) / CP.longitudinalActuatorDelay - a_target_now
|
||||
|
||||
v_target_1sec = interp(CP.longitudinalActuatorDelay + DT_MDL + 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
|
||||
should_stop = (v_target < CP.vEgoStopping and
|
||||
v_target_1sec < CP.vEgoStopping)
|
||||
return a_target, should_stop
|
||||
|
||||
|
||||
class LongitudinalPlanner:
|
||||
def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL):
|
||||
self.CP = CP
|
||||
|
|
@ -142,9 +162,14 @@ class LongitudinalPlanner:
|
|||
|
||||
plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
|
||||
|
||||
|
||||
longitudinalPlan = plan_send.longitudinalPlan
|
||||
longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']
|
||||
longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2']
|
||||
longitudinalPlan.solverExecutionTime = self.mpc.solve_time
|
||||
|
||||
longitudinalPlan.allowBrake = True
|
||||
longitudinalPlan.allowThrottle = True
|
||||
|
||||
longitudinalPlan.speeds = self.v_desired_trajectory.tolist()
|
||||
longitudinalPlan.accels = self.a_desired_trajectory.tolist()
|
||||
|
|
@ -154,6 +179,10 @@ class LongitudinalPlanner:
|
|||
longitudinalPlan.longitudinalPlanSource = self.mpc.source
|
||||
longitudinalPlan.fcw = self.fcw
|
||||
|
||||
longitudinalPlan.solverExecutionTime = self.mpc.solve_time
|
||||
a_target, should_stop = get_accel_from_plan(self.CP, longitudinalPlan)
|
||||
longitudinalPlan.aTarget = a_target
|
||||
longitudinalPlan.shouldStop = should_stop
|
||||
longitudinalPlan.allowBrake = True
|
||||
longitudinalPlan.allowThrottle = True
|
||||
|
||||
pm.send('longitudinalPlan', plan_send)
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
6438bd5edad674c2de3c7e2d126271cb2576383d
|
||||
8737e368e17f859291164286feb4246e00c0b4a5
|
||||
|
|
|
|||
|
|
@ -158,7 +158,6 @@ def ui_thread(addr):
|
|||
# TODO brake is deprecated
|
||||
plot_arr[-1, name_to_arr_idx['computer_brake']] = clip(-sm['carControl'].actuators.accel/4.0, 0.0, 1.0)
|
||||
plot_arr[-1, name_to_arr_idx['v_ego']] = sm['carState'].vEgo
|
||||
plot_arr[-1, name_to_arr_idx['v_pid']] = sm['controlsState'].vPid
|
||||
plot_arr[-1, name_to_arr_idx['v_cruise']] = sm['carState'].cruiseState.speed
|
||||
plot_arr[-1, name_to_arr_idx['a_ego']] = sm['carState'].aEgo
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue