-
Notifications
You must be signed in to change notification settings - Fork 9.5k
/
Copy pathcarstate.py
177 lines (154 loc) · 6.78 KB
/
carstate.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
import os
from common.fingerprints import TOYOTA as CAR
from selfdrive.can.parser import CANParser
from selfdrive.config import Conversions as CV
from common.kalman.simple_kalman import KF1D
import numpy as np
def parse_gear_shifter(can_gear, car_fingerprint):
# TODO: Use values from DBC to parse this field
if car_fingerprint in [CAR.PRIUS, CAR.CAMRYH]:
if can_gear == 0x0:
return "park"
elif can_gear == 0x1:
return "reverse"
elif can_gear == 0x2:
return "neutral"
elif can_gear == 0x3:
return "drive"
elif can_gear == 0x4:
return "brake"
elif car_fingerprint in [CAR.RAV4, CAR.RAV4H,
CAR.LEXUS_RXH, CAR.COROLLA]:
if can_gear == 0x20:
return "park"
elif can_gear == 0x10:
return "reverse"
elif can_gear == 0x8:
return "neutral"
elif can_gear == 0x0:
return "drive"
elif can_gear == 0x1:
return "sport"
return "unknown"
def get_can_parser(CP):
# this function generates lists for signal, messages and initial values
if CP.carFingerprint == CAR.PRIUS:
dbc_f = 'toyota_prius_2017_pt_generated.dbc'
elif CP.carFingerprint == CAR.RAV4H:
dbc_f = 'toyota_rav4_hybrid_2017_pt_generated.dbc'
elif CP.carFingerprint == CAR.RAV4:
dbc_f = 'toyota_rav4_2017_pt_generated.dbc'
elif CP.carFingerprint == CAR.COROLLA:
dbc_f = 'toyota_corolla_2017_pt_generated.dbc'
elif CP.carFingerprint == CAR.LEXUS_RXH:
dbc_f = 'lexus_rx_hybrid_2017_pt_generated.dbc'
elif CP.carFingerprint == CAR.CAMRYH:
dbc_f = 'toyota_camry_hybrid_2018_pt_generated.dbc'
signals = [
# sig_name, sig_address, default
("GEAR", "GEAR_PACKET", 0),
("BRAKE_PRESSED", "BRAKE_MODULE", 0),
("GAS_PEDAL", "GAS_PEDAL", 0),
("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0),
("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0),
("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0),
("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0),
("DOOR_OPEN_FL", "SEATS_DOORS", 1),
("DOOR_OPEN_FR", "SEATS_DOORS", 1),
("DOOR_OPEN_RL", "SEATS_DOORS", 1),
("DOOR_OPEN_RR", "SEATS_DOORS", 1),
("SEATBELT_DRIVER_UNLATCHED", "SEATS_DOORS", 1),
("TC_DISABLED", "ESP_CONTROL", 1),
("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0),
("STEER_FRACTION", "STEER_ANGLE_SENSOR", 0),
("STEER_RATE", "STEER_ANGLE_SENSOR", 0),
("GAS_RELEASED", "PCM_CRUISE", 0),
("CRUISE_STATE", "PCM_CRUISE", 0),
("MAIN_ON", "PCM_CRUISE_2", 0),
("SET_SPEED", "PCM_CRUISE_2", 0),
("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2", 0),
("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0),
("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR", 0),
("TURN_SIGNALS", "STEERING_LEVERS", 3), # 3 is no blinkers
("LKA_STATE", "EPS_STATUS", 0),
("IPAS_STATE", "EPS_STATUS", 1),
("BRAKE_LIGHTS_ACC", "ESP_CONTROL", 0),
("AUTO_HIGH_BEAM", "LIGHT_STALK", 0),
]
checks = [
("BRAKE_MODULE", 40),
("GAS_PEDAL", 33),
("WHEEL_SPEEDS", 80),
("STEER_ANGLE_SENSOR", 80),
("PCM_CRUISE", 33),
("PCM_CRUISE_2", 33),
("STEER_TORQUE_SENSOR", 50),
("EPS_STATUS", 25),
]
return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 0)
class CarState(object):
def __init__(self, CP):
self.CP = CP
self.left_blinker_on = 0
self.right_blinker_on = 0
# initialize can parser
self.car_fingerprint = CP.carFingerprint
# vEgo kalman filter
dt = 0.01
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
# R = 1e3
self.v_ego_kf = KF1D(x0=np.matrix([[0.0], [0.0]]),
A=np.matrix([[1.0, dt], [0.0, 1.0]]),
C=np.matrix([1.0, 0.0]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego = 0.0
def update(self, cp):
# copy can_valid
self.can_valid = cp.can_valid
# update prevs, update must run once per loop
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
self.door_all_closed = not any([cp.vl["SEATS_DOORS"]['DOOR_OPEN_FL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_FR'],
cp.vl["SEATS_DOORS"]['DOOR_OPEN_RL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_RR']])
self.seatbelt = not cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED']
can_gear = cp.vl["GEAR_PACKET"]['GEAR']
self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED']
self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
self.car_gas = self.pedal_gas
self.esp_disabled = cp.vl["ESP_CONTROL"]['TC_DISABLED']
# calc best v_ego estimate, by averaging two opposite corners
self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS
self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS
self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS
self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS
self.v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
# Kalman filter
if abs(self.v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_x = np.matrix([[self.v_wheel], [0.0]])
self.v_ego_raw = self.v_wheel
v_ego_x = self.v_ego_kf.update(self.v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
self.standstill = not self.v_wheel > 0.001
self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
self.gear_shifter = parse_gear_shifter(can_gear, self.car_fingerprint)
self.main_on = cp.vl["PCM_CRUISE_2"]['MAIN_ON']
self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
# we could use the override bit from dbc, but it's triggered at too high torque values
self.steer_override = abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']) > 100
# 2 is standby, 10 is active. TODO: check that everything else is really a faulty state
self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE']
self.steer_error = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [2, 10]
self.ipas_state = cp.vl['EPS_STATUS']['IPAS_STATE']
self.brake_error = 0
self.steer_torque_driver = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']
self.steer_torque_motor = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_EPS']
self.user_brake = 0
self.v_cruise_pcm = cp.vl["PCM_CRUISE_2"]['SET_SPEED']
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
self.gas_pressed = not cp.vl["PCM_CRUISE"]['GAS_RELEASED']
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2
self.brake_lights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or self.brake_pressed)
self.generic_toggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM'])