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.
187 lines
5.9 KiB
187 lines
5.9 KiB
import os
|
|
import time
|
|
from common.realtime import sec_since_boot
|
|
import selfdrive.messaging as messaging
|
|
from selfdrive.car.toyota.carcontroller import CAR
|
|
from selfdrive.can.parser import CANParser
|
|
from selfdrive.config import Conversions as CV
|
|
import numpy as np
|
|
|
|
def parse_gear_shifter(can_gear, car_fingerprint):
|
|
|
|
if car_fingerprint == CAR.PRIUS:
|
|
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 == CAR.RAV4:
|
|
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.dbc'
|
|
signals = [
|
|
("GEAR", 295, 0),
|
|
("BRAKE_PRESSED", 550, 0),
|
|
("GAS_PEDAL", 581, 0),
|
|
]
|
|
checks = [
|
|
(550, 40),
|
|
(581, 33)
|
|
]
|
|
elif CP.carFingerprint == CAR.RAV4:
|
|
dbc_f = 'toyota_rav4_2017_pt.dbc'
|
|
signals = [
|
|
("GEAR", 956, 0x20),
|
|
("BRAKE_PRESSED", 548, 0),
|
|
("GAS_PEDAL", 705, 0),
|
|
]
|
|
checks = [
|
|
(548, 40),
|
|
(705, 33)
|
|
]
|
|
|
|
# TODO: DOORS, GAS_PEDAL, BRAKE_PRESSED for RAV4
|
|
signals += [
|
|
# sig_name, sig_address, default
|
|
("WHEEL_SPEED_FL", 170, 0),
|
|
("WHEEL_SPEED_FR", 170, 0),
|
|
("WHEEL_SPEED_RL", 170, 0),
|
|
("WHEEL_SPEED_RR", 170, 0),
|
|
("DOOR_OPEN_FL", 1568, 1),
|
|
("DOOR_OPEN_FR", 1568, 1),
|
|
("DOOR_OPEN_RL", 1568, 1),
|
|
("DOOR_OPEN_RR", 1568, 1),
|
|
("SEATBELT_DRIVER_UNLATCHED", 1568, 1),
|
|
("TC_DISABLED", 951, 1),
|
|
("STEER_ANGLE", 37, 0),
|
|
("STEER_FRACTION", 37, 0),
|
|
("STEER_RATE", 37, 0),
|
|
("GAS_RELEASED", 466, 0),
|
|
("CRUISE_STATE", 466, 0),
|
|
("MAIN_ON", 467, 0),
|
|
("SET_SPEED", 467, 0),
|
|
("LOW_SPEED_LOCKOUT", 467, 0),
|
|
("STEER_TORQUE_DRIVER", 608, 0),
|
|
("STEER_TORQUE_EPS", 608, 0),
|
|
("TURN_SIGNALS", 1556, 3), # 3 is no blinkers
|
|
("LKA_STATE", 610, 0),
|
|
]
|
|
|
|
checks += [
|
|
(170, 80),
|
|
(37, 80),
|
|
(466, 33),
|
|
(467, 33),
|
|
(608, 50),
|
|
(610, 25),
|
|
]
|
|
|
|
return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 0)
|
|
|
|
|
|
class CarState(object):
|
|
def __init__(self, CP, logcan):
|
|
|
|
self.CP = CP
|
|
self.left_blinker_on = 0
|
|
self.right_blinker_on = 0
|
|
|
|
# initialize can parser
|
|
self.cp = get_can_parser(CP)
|
|
self.car_fingerprint = CP.carFingerprint
|
|
|
|
# vEgo kalman filter
|
|
dt = 0.01
|
|
self.v_ego_x = np.matrix([[0.0], [0.0]])
|
|
self.v_ego_A = np.matrix([[1.0, dt], [0.0, 1.0]])
|
|
self.v_ego_C = np.matrix([1.0, 0.0])
|
|
self.v_ego_Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
|
|
self.v_ego_R = 1e3
|
|
# import control
|
|
# (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R)
|
|
# self.v_ego_K = np.transpose(K)
|
|
self.v_ego_K = np.matrix([[0.12287673], [0.29666309]])
|
|
|
|
def update(self):
|
|
cp = self.cp
|
|
cp.update(int(sec_since_boot() * 1e9), False)
|
|
|
|
# copy can_valid
|
|
self.can_valid = cp.can_valid
|
|
|
|
if self.car_fingerprint == CAR.PRIUS:
|
|
can_gear = cp.vl[295]['GEAR']
|
|
self.brake_pressed = cp.vl[550]['BRAKE_PRESSED']
|
|
self.pedal_gas = cp.vl[581]['GAS_PEDAL']
|
|
elif self.car_fingerprint == CAR.RAV4:
|
|
can_gear = cp.vl[956]['GEAR']
|
|
self.brake_pressed = cp.vl[548]['BRAKE_PRESSED']
|
|
self.pedal_gas = cp.vl[705]['GAS_PEDAL']
|
|
|
|
# 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
|
|
|
|
# ******************* parse out can *******************
|
|
self.door_all_closed = not any([cp.vl[1568]['DOOR_OPEN_FL'], cp.vl[1568]['DOOR_OPEN_FR'],
|
|
cp.vl[1568]['DOOR_OPEN_RL'], cp.vl[1568]['DOOR_OPEN_RR']])
|
|
self.seatbelt = not cp.vl[1568]['SEATBELT_DRIVER_UNLATCHED']
|
|
# whitelist instead of blacklist, safer at the expense of disengages
|
|
self.steer_error = False
|
|
self.brake_error = 0
|
|
self.esp_disabled = cp.vl[951]['TC_DISABLED']
|
|
# calc best v_ego estimate, by averaging two opposite corners
|
|
self.v_wheel_fl = cp.vl[170]['WHEEL_SPEED_FL']
|
|
self.v_wheel_fr = cp.vl[170]['WHEEL_SPEED_FR']
|
|
self.v_wheel_rl = cp.vl[170]['WHEEL_SPEED_RL']
|
|
self.v_wheel_rr = cp.vl[170]['WHEEL_SPEED_RR']
|
|
self.v_wheel = (
|
|
cp.vl[170]['WHEEL_SPEED_FL'] + cp.vl[170]['WHEEL_SPEED_FR'] +
|
|
cp.vl[170]['WHEEL_SPEED_RL'] + cp.vl[170]['WHEEL_SPEED_RR']) / 4. * CV.KPH_TO_MS
|
|
|
|
# Kalman filter
|
|
self.v_ego_x = np.dot((self.v_ego_A - np.dot(self.v_ego_K, self.v_ego_C)), self.v_ego_x) + np.dot(self.v_ego_K, self.v_wheel)
|
|
self.v_ego_raw = self.v_wheel
|
|
self.v_ego = float(self.v_ego_x[0])
|
|
self.a_ego = float(self.v_ego_x[1])
|
|
self.standstill = not self.v_wheel > 0.001
|
|
|
|
self.angle_steers = cp.vl[37]['STEER_ANGLE'] + cp.vl[37]['STEER_FRACTION']
|
|
self.angle_steers_rate = cp.vl[37]['STEER_RATE']
|
|
self.gear_shifter = parse_gear_shifter(can_gear, self.car_fingerprint)
|
|
self.main_on = cp.vl[467]['MAIN_ON']
|
|
self.left_blinker_on = cp.vl[1556]['TURN_SIGNALS'] == 1
|
|
self.right_blinker_on = cp.vl[1556]['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[608]['STEER_TORQUE_DRIVER']) > 100
|
|
self.steer_error = cp.vl[610]['LKA_STATE'] == 50
|
|
self.steer_torque_driver = cp.vl[608]['STEER_TORQUE_DRIVER']
|
|
self.steer_torque_motor = cp.vl[608]['STEER_TORQUE_EPS']
|
|
|
|
self.user_brake = 0
|
|
self.v_cruise_pcm = cp.vl[467]['SET_SPEED']
|
|
self.pcm_acc_status = cp.vl[466]['CRUISE_STATE']
|
|
self.car_gas = self.pedal_gas
|
|
self.gas_pressed = not cp.vl[466]['GAS_RELEASED']
|
|
self.low_speed_lockout = cp.vl[467]['LOW_SPEED_LOCKOUT'] == 2
|
|
|