forked from move-fast/openpilot
-
Notifications
You must be signed in to change notification settings - Fork 31
/
interface.py
executable file
·287 lines (241 loc) · 11.5 KB
/
interface.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
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
#!/usr/bin/env python3
from cereal import car
from math import fabs
from panda import Panda
from common.conversions import Conversions as CV
from selfdrive.car import STD_CARGO_KG, create_button_event, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
from selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams, EV_CAR, CAMERA_ACC_CAR
from selfdrive.car.interfaces import CarInterfaceBase
ButtonType = car.CarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
GearShifter = car.CarState.GearShifter
TransmissionType = car.CarParams.TransmissionType
NetworkLocation = car.CarParams.NetworkLocation
BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise,
CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel}
class CarInterface(CarInterfaceBase):
@staticmethod
def get_pid_accel_limits(CP, current_speed, cruise_speed):
params = CarControllerParams()
return params.ACCEL_MIN, params.ACCEL_MAX
# Determined by iteratively plotting and minimizing error for f(angle, speed) = steer.
@staticmethod
def get_steer_feedforward_volt(desired_angle, v_ego):
desired_angle *= 0.02904609
sigmoid = desired_angle / (1 + fabs(desired_angle))
return 0.10006696 * sigmoid * (v_ego + 3.12485927)
@staticmethod
def get_steer_feedforward_acadia(desired_angle, v_ego):
desired_angle *= 0.09760208
sigmoid = desired_angle / (1 + fabs(desired_angle))
return 0.04689655 * sigmoid * (v_ego + 10.028217)
def get_steer_feedforward_function(self):
if self.CP.carFingerprint == CAR.VOLT:
return self.get_steer_feedforward_volt
elif self.CP.carFingerprint == CAR.ACADIA:
return self.get_steer_feedforward_acadia
else:
return CarInterfaceBase.get_steer_feedforward_default
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False):
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
ret.carName = "gm"
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)]
ret.autoResumeSng = False
if candidate in EV_CAR:
ret.transmissionType = TransmissionType.direct
else:
ret.transmissionType = TransmissionType.automatic
if candidate in CAMERA_ACC_CAR:
ret.openpilotLongitudinalControl = False
ret.networkLocation = NetworkLocation.fwdCamera
ret.radarOffCan = True # no radar
ret.pcmCruise = True
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM
else: # ASCM, OBD-II harness
ret.openpilotLongitudinalControl = True
ret.networkLocation = NetworkLocation.gateway
ret.radarOffCan = False
ret.pcmCruise = False # stock non-adaptive cruise control is kept off
# 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
# added to selfdrive/car/tests/routes.py, we can remove it from this list.
ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU, CAR.BUICK_REGAL}
# Start with a baseline tuning for all GM vehicles. Override tuning as needed in each model section below.
ret.minSteerSpeed = 10 * CV.KPH_TO_MS
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]]
ret.lateralTuning.pid.kf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594
ret.steerActuatorDelay = 0.1 # Default delay, not measured yet
tire_stiffness_factor = 0.444 # not optimized yet
ret.longitudinalTuning.kpBP = [5., 35.]
ret.longitudinalTuning.kpV = [2.4, 1.5]
ret.longitudinalTuning.kiBP = [0.]
ret.longitudinalTuning.kiV = [0.36]
ret.steerLimitTimer = 0.4
ret.radarTimeStep = 0.0667 # GM radar runs at 15Hz instead of standard 20Hz
# supports stop and go, but initial engage must (conservatively) be above 18mph
ret.minEnableSpeed = 18 * CV.MPH_TO_MS
if candidate == CAR.VOLT:
ret.mass = 1607. + STD_CARGO_KG
ret.wheelbase = 2.69
ret.steerRatio = 17.7 # Stock 15.7, LiveParameters
tire_stiffness_factor = 0.469 # Stock Michelin Energy Saver A/S, LiveParameters
ret.centerToFront = ret.wheelbase * 0.45 # Volt Gen 1, TODO corner weigh
ret.lateralTuning.pid.kpBP = [0., 40.]
ret.lateralTuning.pid.kpV = [0., 0.17]
ret.lateralTuning.pid.kiBP = [0.]
ret.lateralTuning.pid.kiV = [0.]
ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_volt()
ret.steerActuatorDelay = 0.2
elif candidate == CAR.MALIBU:
ret.mass = 1496. + STD_CARGO_KG
ret.wheelbase = 2.83
ret.steerRatio = 15.8
ret.centerToFront = ret.wheelbase * 0.4 # wild guess
elif candidate == CAR.HOLDEN_ASTRA:
ret.mass = 1363. + STD_CARGO_KG
ret.wheelbase = 2.662
# Remaining parameters copied from Volt for now
ret.centerToFront = ret.wheelbase * 0.4
ret.steerRatio = 15.7
elif candidate == CAR.ACADIA:
ret.minEnableSpeed = -1. # engage speed is decided by pcm
ret.mass = 4353. * CV.LB_TO_KG + STD_CARGO_KG
ret.wheelbase = 2.86
ret.steerRatio = 14.4 # end to end is 13.46
ret.centerToFront = ret.wheelbase * 0.4
ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_acadia()
ret.longitudinalActuatorDelayUpperBound = 0.5 # large delay to initially start braking
elif candidate == CAR.BUICK_REGAL:
ret.mass = 3779. * CV.LB_TO_KG + STD_CARGO_KG # (3849+3708)/2
ret.wheelbase = 2.83 # 111.4 inches in meters
ret.steerRatio = 14.4 # guess for tourx
ret.centerToFront = ret.wheelbase * 0.4 # guess for tourx
elif candidate == CAR.CADILLAC_ATS:
ret.mass = 1601. + STD_CARGO_KG
ret.wheelbase = 2.78
ret.steerRatio = 15.3
ret.centerToFront = ret.wheelbase * 0.49
elif candidate == CAR.ESCALADE_ESV:
ret.minEnableSpeed = -1. # engage speed is decided by pcm
ret.mass = 2739. + STD_CARGO_KG
ret.wheelbase = 3.302
ret.steerRatio = 17.3
ret.centerToFront = ret.wheelbase * 0.49
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[10., 41.0], [10., 41.0]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.13, 0.24], [0.01, 0.02]]
ret.lateralTuning.pid.kf = 0.000045
tire_stiffness_factor = 1.0
elif candidate == CAR.BOLT_EUV:
ret.minEnableSpeed = -1
ret.mass = 1669. + STD_CARGO_KG
ret.wheelbase = 2.675
ret.steerRatio = 16.8
ret.centerToFront = ret.wheelbase * 0.4
tire_stiffness_factor = 1.0
ret.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.SILVERADO:
ret.minEnableSpeed = -1
ret.mass = 2200. + STD_CARGO_KG
ret.wheelbase = 3.75
ret.steerRatio = 16.3
ret.centerToFront = ret.wheelbase * 0.5
tire_stiffness_factor = 1.0
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor)
return ret
# returns a car.CarState
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback)
ret.madsEnabled = self.CS.madsEnabled
ret.accEnabled = self.CS.accEnabled
ret.leftBlinkerOn = self.CS.leftBlinkerOn
ret.rightBlinkerOn = self.CS.rightBlinkerOn
ret.belowLaneChangeSpeed = self.CS.belowLaneChangeSpeed
buttonEvents = []
if ret.cruiseState.enabled and not self.CS.out.cruiseState.enabled:
be = car.CarState.ButtonEvent.new_message()
be.pressed = False
be.type = ButtonType.setCruise
buttonEvents.append(be)
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.unknown
if self.CS.cruise_buttons != CruiseButtons.UNPRESS:
be.pressed = True
but = self.CS.cruise_buttons
else:
be.pressed = False
but = self.CS.prev_cruise_buttons
if but == CruiseButtons.RES_ACCEL:
be.type = ButtonType.accelCruise
elif but == CruiseButtons.DECEL_SET:
be.type = ButtonType.decelCruise
elif but == CruiseButtons.CANCEL:
be.type = ButtonType.cancel
if be.type == ButtonType.accelCruise and (ret.cruiseState.enabled and ret.standstill):
be.type = ButtonType.unknown
buttonEvents.append(be)
# MADS BUTTON
if self.CS.out.madsEnabled != self.CS.madsEnabled:
be = car.CarState.ButtonEvent.new_message()
be.pressed = True
be.type = ButtonType.altButton1
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
extraGears = []
if not self.CS.CP.openpilotLongitudinalControl:
extraGears = [car.CarState.GearShifter.sport, car.CarState.GearShifter.low, car.CarState.GearShifter.eco, car.CarState.GearShifter.manumatic]
events = self.create_common_events(ret, extra_gears=extraGears, pcm_enable=False)
if ret.vEgo < self.CP.minEnableSpeed:
events.add(EventName.belowEngageSpeed)
if ret.cruiseState.standstill:
events.add(EventName.resumeRequired)
if ret.vEgo < self.CP.minSteerSpeed:
events.add(car.CarEvent.EventName.belowSteerSpeed)
self.CS.disengageByBrake = self.CS.disengageByBrake or ret.disengageByBrake
enable_pressed = False
enable_from_brake = False
if self.CS.disengageByBrake and not ret.brakePressed and not ret.brakeHoldActive and not ret.parkingBrake and self.CS.madsEnabled:
enable_pressed = True
enable_from_brake = True
if not ret.brakePressed and not ret.brakeHoldActive and not ret.parkingBrake:
self.CS.disengageByBrake = False
ret.disengageByBrake = False
for b in ret.buttonEvents:
# do enable on both accel and decel buttons
if b.type in (ButtonType.accelCruise, ButtonType.decelCruise, ButtonType.setCruise) and not b.pressed:
enable_pressed = True
# do disable on MADS button if ACC is disabled
if b.type == ButtonType.altButton1 and b.pressed:
if not self.CS.madsEnabled: # disabled MADS
if not ret.cruiseState.enabled:
events.add(EventName.buttonCancel)
else:
events.add(EventName.manualSteeringRequired)
else: # enabled MADS
if not ret.cruiseState.enabled:
enable_pressed = True
# do disable on button down
if b.type == ButtonType.cancel and b.pressed:
if not self.CS.madsEnabled:
events.add(EventName.buttonCancel)
elif ret.cruiseState.enabled:
events.add(EventName.manualLongitudinalRequired)
if (ret.cruiseState.enabled or self.CS.madsEnabled) and enable_pressed:
if enable_from_brake:
events.add(EventName.silentButtonEnable)
else:
events.add(EventName.buttonEnable)
ret.events = events.to_msg()
return ret
def apply(self, c):
return self.CC.update(c, self.CS)