/
longcontrol.py
130 lines (103 loc) · 5.5 KB
/
longcontrol.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
from cereal import car
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, apply_deadzone
from openpilot.selfdrive.controls.lib.pid import PIDController
from openpilot.selfdrive.modeld.constants import ModelConstants
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
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
self.long_control_state = LongCtrlState.off # initialized to 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"""
self.pid.reset()
self.v_pid = v_pid
def update(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, ModelConstants.T_IDXS[:CONTROL_N], speeds)
a_target_now = interp(t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], long_plan.accels)
v_target_lower = interp(self.CP.longitudinalActuatorDelayLowerBound + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], speeds)
a_target_lower = 2 * (v_target_lower - v_target_now) / self.CP.longitudinalActuatorDelayLowerBound - a_target_now
v_target_upper = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], speeds)
a_target_upper = 2 * (v_target_upper - v_target_now) / self.CP.longitudinalActuatorDelayUpperBound - a_target_now
v_target = min(v_target_lower, v_target_upper)
a_target = min(a_target_lower, a_target_upper)
v_target_1sec = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan + 1.0, ModelConstants.T_IDXS[:CONTROL_N], 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,
CS.cruiseState.standstill)
if self.long_control_state == LongCtrlState.off:
self.reset(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(CS.vEgo)
elif self.long_control_state == LongCtrlState.starting:
output_accel = self.CP.startAccel
self.reset(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