You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
275 lines
9.6 KiB
275 lines
9.6 KiB
import numpy as np
|
|
|
|
import selfdrive.messaging as messaging
|
|
from selfdrive.boardd.boardd import can_capnp_to_can_list_old, can_capnp_to_can_list
|
|
from selfdrive.controls.lib.can_parser import CANParser
|
|
from selfdrive.controls.lib.fingerprints import fingerprints
|
|
from selfdrive.config import VehicleParams
|
|
from common.realtime import sec_since_boot
|
|
|
|
|
|
def get_can_parser(civic, brake_only):
|
|
# this function generates lists for signal, messages and initial values
|
|
if civic:
|
|
dbc_f = 'honda_civic_touring_2016_can.dbc'
|
|
signals = [
|
|
("XMISSION_SPEED", 0x158, 0),
|
|
("WHEEL_SPEED_FL", 0x1d0, 0),
|
|
("WHEEL_SPEED_FR", 0x1d0, 0),
|
|
("WHEEL_SPEED_RL", 0x1d0, 0),
|
|
("STEER_ANGLE", 0x14a, 0),
|
|
("STEER_TORQUE_SENSOR", 0x18f, 0),
|
|
("GEAR", 0x191, 0),
|
|
("WHEELS_MOVING", 0x1b0, 1),
|
|
("DOOR_OPEN_FL", 0x405, 1),
|
|
("DOOR_OPEN_FR", 0x405, 1),
|
|
("DOOR_OPEN_RL", 0x405, 1),
|
|
("DOOR_OPEN_RR", 0x405, 1),
|
|
("CRUISE_SPEED_PCM", 0x324, 0),
|
|
("SEATBELT_DRIVER_LAMP", 0x305, 1),
|
|
("SEATBELT_DRIVER_LATCHED", 0x305, 0),
|
|
("BRAKE_PRESSED", 0x17c, 0),
|
|
("CAR_GAS", 0x130, 0),
|
|
("CRUISE_BUTTONS", 0x296, 0),
|
|
("ESP_DISABLED", 0x1a4, 1),
|
|
("HUD_LEAD", 0x30c, 0),
|
|
("USER_BRAKE", 0x1a4, 0),
|
|
("STEER_STATUS", 0x18f, 5),
|
|
("WHEEL_SPEED_RR", 0x1d0, 0),
|
|
("BRAKE_ERROR_1", 0x1b0, 1),
|
|
("BRAKE_ERROR_2", 0x1b0, 1),
|
|
("GEAR_SHIFTER", 0x191, 0),
|
|
("MAIN_ON", 0x326, 0),
|
|
("ACC_STATUS", 0x17c, 0),
|
|
("PEDAL_GAS", 0x17c, 0),
|
|
("CRUISE_SETTING", 0x296, 0),
|
|
("LEFT_BLINKER", 0x326, 0),
|
|
("RIGHT_BLINKER", 0x326, 0),
|
|
("COUNTER", 0x324, 0),
|
|
]
|
|
checks = [
|
|
(0x14a, 100),
|
|
(0x158, 100),
|
|
(0x17c, 100),
|
|
(0x191, 100),
|
|
(0x1a4, 50),
|
|
(0x326, 10),
|
|
(0x1b0, 50),
|
|
(0x1d0, 50),
|
|
(0x305, 10),
|
|
(0x324, 10),
|
|
(0x405, 3),
|
|
]
|
|
|
|
else:
|
|
dbc_f = 'acura_ilx_2016_can.dbc'
|
|
signals = [
|
|
("XMISSION_SPEED", 0x158, 0),
|
|
("WHEEL_SPEED_FL", 0x1d0, 0),
|
|
("WHEEL_SPEED_FR", 0x1d0, 0),
|
|
("WHEEL_SPEED_RL", 0x1d0, 0),
|
|
("STEER_ANGLE", 0x156, 0),
|
|
("STEER_TORQUE_SENSOR", 0x18f, 0),
|
|
("GEAR", 0x1a3, 0),
|
|
("WHEELS_MOVING", 0x1b0, 1),
|
|
("DOOR_OPEN_FL", 0x405, 1),
|
|
("DOOR_OPEN_FR", 0x405, 1),
|
|
("DOOR_OPEN_RL", 0x405, 1),
|
|
("DOOR_OPEN_RR", 0x405, 1),
|
|
("CRUISE_SPEED_PCM", 0x324, 0),
|
|
("SEATBELT_DRIVER_LAMP", 0x305, 1),
|
|
("SEATBELT_DRIVER_LATCHED", 0x305, 0),
|
|
("BRAKE_PRESSED", 0x17c, 0),
|
|
("CAR_GAS", 0x130, 0),
|
|
("CRUISE_BUTTONS", 0x1a6, 0),
|
|
("ESP_DISABLED", 0x1a4, 1),
|
|
("HUD_LEAD", 0x30c, 0),
|
|
("USER_BRAKE", 0x1a4, 0),
|
|
("STEER_STATUS", 0x18f, 5),
|
|
("WHEEL_SPEED_RR", 0x1d0, 0),
|
|
("BRAKE_ERROR_1", 0x1b0, 1),
|
|
("BRAKE_ERROR_2", 0x1b0, 1),
|
|
("GEAR_SHIFTER", 0x1a3, 0),
|
|
("MAIN_ON", 0x1a6, 0),
|
|
("ACC_STATUS", 0x17c, 0),
|
|
("PEDAL_GAS", 0x17c, 0),
|
|
("CRUISE_SETTING", 0x1a6, 0),
|
|
("LEFT_BLINKER", 0x294, 0),
|
|
("RIGHT_BLINKER", 0x294, 0),
|
|
("COUNTER", 0x324, 0),
|
|
]
|
|
checks = [
|
|
(0x156, 100),
|
|
(0x158, 100),
|
|
(0x17c, 100),
|
|
(0x1a3, 50),
|
|
(0x1a4, 50),
|
|
(0x1a6, 50),
|
|
(0x1b0, 50),
|
|
(0x1d0, 50),
|
|
(0x305, 10),
|
|
(0x324, 10),
|
|
(0x405, 3),
|
|
]
|
|
|
|
# add gas interceptor reading if we are using it
|
|
if not brake_only:
|
|
signals.append(("INTERCEPTOR_GAS", 0x201, 0))
|
|
checks.append((0x201, 50))
|
|
|
|
return CANParser(dbc_f, signals, checks)
|
|
|
|
def fingerprint(logcan):
|
|
print "waiting for fingerprint..."
|
|
brake_only = True
|
|
|
|
finger = {}
|
|
st = None
|
|
while 1:
|
|
possible_cars = []
|
|
for a in messaging.drain_sock(logcan, wait_for_one=True):
|
|
if st is None:
|
|
st = sec_since_boot()
|
|
for adr, _, msg, idx in can_capnp_to_can_list(a):
|
|
# pedal
|
|
if adr == 0x201 and idx == 0:
|
|
brake_only = False
|
|
if idx == 0:
|
|
finger[adr] = len(msg)
|
|
|
|
# check for a single match
|
|
for f in fingerprints:
|
|
is_possible = True
|
|
for adr in finger:
|
|
# confirm all messages we have seen match
|
|
if adr not in fingerprints[f] or fingerprints[f][adr] != finger[adr]:
|
|
#print "mismatch", f, adr
|
|
is_possible = False
|
|
break
|
|
if is_possible:
|
|
possible_cars.append(f)
|
|
|
|
# if we only have one car choice and it's been 100ms since we got our first message, exit
|
|
if len(possible_cars) == 1 and st is not None and (sec_since_boot()-st) > 0.1:
|
|
break
|
|
elif len(possible_cars) == 0:
|
|
raise Exception("car doesn't match any fingerprints")
|
|
|
|
print "fingerprinted", possible_cars[0]
|
|
return brake_only, possible_cars[0]
|
|
|
|
class CarState(object):
|
|
def __init__(self, logcan):
|
|
self.torque_mod = False
|
|
self.brake_only, self.car_type = fingerprint(logcan)
|
|
|
|
# assuming if you have a pedal interceptor you also have a torque mod
|
|
if not self.brake_only:
|
|
self.torque_mod = True
|
|
|
|
if self.car_type == "HONDA CIVIC 2016 TOURING":
|
|
self.civic = True
|
|
elif self.car_type == "ACURA ILX 2016 ACURAWATCH PLUS":
|
|
self.civic = False
|
|
else:
|
|
raise ValueError("unsupported car %s" % self.car_type)
|
|
|
|
# initialize can parser
|
|
self.cp = get_can_parser(self.civic, self.brake_only)
|
|
|
|
self.user_gas, self.user_gas_pressed = 0., 0
|
|
|
|
self.cruise_buttons = 0
|
|
self.cruise_setting = 0
|
|
self.blinker_on = 0
|
|
|
|
# TODO: actually make this work
|
|
self.a_ego = 0.
|
|
|
|
# speed in UI is shown as few % higher
|
|
self.ui_speed_fudge = 1.01 if self.civic else 1.025
|
|
|
|
# load vehicle params
|
|
self.VP = VehicleParams(self.civic)
|
|
|
|
def update(self, logcan):
|
|
# ******************* do can recv *******************
|
|
can_pub_main = []
|
|
canMonoTimes = []
|
|
for a in messaging.drain_sock(logcan):
|
|
canMonoTimes.append(a.logMonoTime)
|
|
can_pub_main.extend(can_capnp_to_can_list_old(a, [0,2]))
|
|
|
|
cp = self.cp
|
|
cp.update_can(can_pub_main)
|
|
|
|
# copy can_valid
|
|
self.can_valid = cp.can_valid
|
|
|
|
# car params
|
|
v_weight_v = [0., 1. ] # don't trust smooth speed at low values to avoid premature zero snapping
|
|
v_weight_bp = [1., 6.] # smooth blending, below ~0.6m/s the smooth speed snaps to zero
|
|
|
|
# update prevs, update must run once per loop
|
|
self.prev_cruise_buttons = self.cruise_buttons
|
|
self.prev_cruise_setting = self.cruise_setting
|
|
self.prev_blinker_on = self.blinker_on
|
|
|
|
# ******************* parse out can *******************
|
|
self.door_all_closed = not any([cp.vl[0x405]['DOOR_OPEN_FL'], cp.vl[0x405]['DOOR_OPEN_FR'],
|
|
cp.vl[0x405]['DOOR_OPEN_RL'], cp.vl[0x405]['DOOR_OPEN_RR']])
|
|
self.seatbelt = not cp.vl[0x305]['SEATBELT_DRIVER_LAMP'] and cp.vl[0x305]['SEATBELT_DRIVER_LATCHED']
|
|
# error 2 = temporary
|
|
# error 4 = temporary, hit a bump
|
|
# error 5 (permanent)
|
|
# error 6 = temporary
|
|
# error 7 (permanent)
|
|
#self.steer_error = cp.vl[0x18F]['STEER_STATUS'] in [5,7]
|
|
# whitelist instead of blacklist, safer at the expense of disengages
|
|
self.steer_error = cp.vl[0x18F]['STEER_STATUS'] not in [0,2,4,6]
|
|
self.steer_not_allowed = cp.vl[0x18F]['STEER_STATUS'] != 0
|
|
if cp.vl[0x18F]['STEER_STATUS'] != 0:
|
|
print cp.vl[0x18F]['STEER_STATUS']
|
|
self.brake_error = cp.vl[0x1B0]['BRAKE_ERROR_1'] or cp.vl[0x1B0]['BRAKE_ERROR_2']
|
|
self.esp_disabled = cp.vl[0x1A4]['ESP_DISABLED']
|
|
# calc best v_ego estimate, by averaging two opposite corners
|
|
self.v_wheel = (
|
|
cp.vl[0x1D0]['WHEEL_SPEED_FL'] + cp.vl[0x1D0]['WHEEL_SPEED_FR'] +
|
|
cp.vl[0x1D0]['WHEEL_SPEED_RL'] + cp.vl[0x1D0]['WHEEL_SPEED_RR']) / 4.
|
|
# blend in transmission speed at low speed, since it has more low speed accuracy
|
|
self.v_weight = np.interp(self.v_wheel, v_weight_bp, v_weight_v)
|
|
self.v_ego = (1. - self.v_weight) * cp.vl[0x158]['XMISSION_SPEED'] + self.v_weight * self.v_wheel
|
|
if not self.brake_only:
|
|
self.user_gas = cp.vl[0x201]['INTERCEPTOR_GAS']
|
|
self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
|
|
#print user_gas, user_gas_pressed
|
|
if self.civic:
|
|
self.gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
|
|
self.angle_steers = cp.vl[0x14A]['STEER_ANGLE']
|
|
self.gear = 0 # TODO: civic has CVT... needs rev engineering
|
|
self.cruise_setting = cp.vl[0x296]['CRUISE_SETTING']
|
|
self.cruise_buttons = cp.vl[0x296]['CRUISE_BUTTONS']
|
|
self.main_on = cp.vl[0x326]['MAIN_ON']
|
|
self.gear_shifter_valid = self.gear_shifter in [1,8] # TODO: 1/P allowed for debug
|
|
self.blinker_on = cp.vl[0x326]['LEFT_BLINKER'] or cp.vl[0x326]['RIGHT_BLINKER']
|
|
else:
|
|
self.gear_shifter = cp.vl[0x1A3]['GEAR_SHIFTER']
|
|
self.angle_steers = cp.vl[0x156]['STEER_ANGLE']
|
|
self.gear = cp.vl[0x1A3]['GEAR']
|
|
self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING']
|
|
self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS']
|
|
self.main_on = cp.vl[0x1A6]['MAIN_ON']
|
|
self.gear_shifter_valid = self.gear_shifter in [1,4] # TODO: 1/P allowed for debug
|
|
self.blinker_on = cp.vl[0x294]['LEFT_BLINKER'] or cp.vl[0x294]['RIGHT_BLINKER']
|
|
self.car_gas = cp.vl[0x130]['CAR_GAS']
|
|
self.brake_pressed = cp.vl[0x17C]['BRAKE_PRESSED']
|
|
self.user_brake = cp.vl[0x1A4]['USER_BRAKE']
|
|
self.standstill = not cp.vl[0x1B0]['WHEELS_MOVING']
|
|
self.steer_override = abs(cp.vl[0x18F]['STEER_TORQUE_SENSOR']) > 1200
|
|
self.v_cruise_pcm = cp.vl[0x324]['CRUISE_SPEED_PCM']
|
|
self.pcm_acc_status = cp.vl[0x17C]['ACC_STATUS']
|
|
self.pedal_gas = cp.vl[0x17C]['PEDAL_GAS']
|
|
self.hud_lead = cp.vl[0x30C]['HUD_LEAD']
|
|
self.counter_pcm = cp.vl[0x324]['COUNTER']
|
|
|
|
return canMonoTimes
|
|
|