def update(self, cp):
ret = car.CarState.new_message()
ret.wheelSpeeds = self.get_wheel_speeds(
cp.vl["WheelSpeed_CG1"]["WhlFl_W_Meas"],
cp.vl["WheelSpeed_CG1"]["WhlFr_W_Meas"],
cp.vl["WheelSpeed_CG1"]["WhlRl_W_Meas"],
cp.vl["WheelSpeed_CG1"]["WhlRr_W_Meas"],
unit=WHEEL_RADIUS,
)
ret.vEgoRaw = mean([
ret.wheelSpeeds.rr, ret.wheelSpeeds.rl, ret.wheelSpeeds.fr,
ret.wheelSpeeds.fl
])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = not ret.vEgoRaw > 0.001
ret.steeringAngleDeg = cp.vl["Steering_Wheel_Data_CG1"][
"SteWhlRelInit_An_Sns"]
ret.steeringPressed = not cp.vl["Lane_Keep_Assist_Status"][
"LaHandsOff_B_Actl"]
ret.steerFaultPermanent = cp.vl["Lane_Keep_Assist_Status"][
"LaActDeny_B_Actl"] == 1
ret.cruiseState.speed = cp.vl["Cruise_Status"][
"Set_Speed"] * CV.MPH_TO_MS
ret.cruiseState.enabled = not (cp.vl["Cruise_Status"]["Cruise_State"]
in (0, 3))
ret.cruiseState.available = cp.vl["Cruise_Status"]["Cruise_State"] != 0
ret.gas = cp.vl["EngineData_14"]["ApedPosScal_Pc_Actl"] / 100.
ret.gasPressed = ret.gas > 1e-6
ret.brakePressed = bool(cp.vl["Cruise_Status"]["Brake_Drv_Appl"])
ret.genericToggle = bool(cp.vl["Steering_Buttons"]["Dist_Incr"])
# TODO: we also need raw driver torque, needed for Assisted Lane Change
self.lkas_state = cp.vl["Lane_Keep_Assist_Status"]["LaActAvail_D_Actl"]
return ret
def update(self, pt_cp, loopback_cp):
ret = car.CarState.new_message()
self.prev_cruise_buttons = self.cruise_buttons
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"]
ret.wheelSpeeds = self.get_wheel_speeds(
pt_cp.vl["EBCMWheelSpdFront"]["FLWheelSpd"],
pt_cp.vl["EBCMWheelSpdFront"]["FRWheelSpd"],
pt_cp.vl["EBCMWheelSpdRear"]["RLWheelSpd"],
pt_cp.vl["EBCMWheelSpdRear"]["RRWheelSpd"],
)
ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.01
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL"]["PRNDL"], None))
# Brake pedal's potentiometer returns near-zero reading even when pedal is not pressed.
ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] / 0xd0
ret.brakePressed = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] >= 10
# Regen braking is braking
if self.car_fingerprint == CAR.VOLT:
ret.brakePressed = ret.brakePressed or pt_cp.vl["EBCMRegenPaddle"]["RegenPaddle"] != 0
ret.gas = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254.
ret.gasPressed = ret.gas > 1e-5
ret.steeringAngleDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelAngle"]
ret.steeringRateDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelRate"]
ret.steeringTorque = pt_cp.vl["PSCMStatus"]["LKADriverAppldTrq"]
ret.steeringTorqueEps = pt_cp.vl["PSCMStatus"]["LKATorqueDelivered"]
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
self.lka_steering_cmd_counter = loopback_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"]
# 0 inactive, 1 active, 2 temporarily limited, 3 failed
self.lkas_status = pt_cp.vl["PSCMStatus"]["LKATorqueDeliveredStatus"]
ret.steerFaultTemporary = self.lkas_status == 2
ret.steerFaultPermanent = self.lkas_status == 3
# 1 - open, 0 - closed
ret.doorOpen = (pt_cp.vl["BCMDoorBeltStatus"]["FrontLeftDoor"] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]["FrontRightDoor"] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]["RearLeftDoor"] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]["RearRightDoor"] == 1)
# 1 - latched
ret.seatbeltUnlatched = pt_cp.vl["BCMDoorBeltStatus"]["LeftSeatBelt"] == 0
ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 1
ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2
ret.parkingBrake = pt_cp.vl["VehicleIgnitionAlt"]["ParkBrake"] == 1
ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0
ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1
ret.accFaulted = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.FAULTED
ret.cruiseState.enabled = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] != AccState.OFF
ret.cruiseState.standstill = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.STANDSTILL
return ret
def update(self, cp):
ret = car.CarState.new_message()
ret.wheelSpeeds.rr = cp.vl["WheelSpeed_CG1"][
'WhlRr_W_Meas'] * WHEEL_RADIUS
ret.wheelSpeeds.rl = cp.vl["WheelSpeed_CG1"][
'WhlRl_W_Meas'] * WHEEL_RADIUS
ret.wheelSpeeds.fr = cp.vl["WheelSpeed_CG1"][
'WhlFr_W_Meas'] * WHEEL_RADIUS
ret.wheelSpeeds.fl = cp.vl["WheelSpeed_CG1"][
'WhlFl_W_Meas'] * WHEEL_RADIUS
ret.vEgoRaw = mean([
ret.wheelSpeeds.rr, ret.wheelSpeeds.rl, ret.wheelSpeeds.fr,
ret.wheelSpeeds.fl
])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = not ret.vEgoRaw > 0.001
ret.steeringAngle = cp.vl["Steering_Wheel_Data_CG1"][
'SteWhlRelInit_An_Sns']
ret.steeringPressed = not cp.vl["Lane_Keep_Assist_Status"][
'LaHandsOff_B_Actl']
ret.cruiseState.speed = cp.vl["Cruise_Status"][
'Set_Speed'] * CV.MPH_TO_MS
ret.cruiseState.enabled = not (cp.vl["Cruise_Status"]['Cruise_State']
in [0, 3])
ret.cruiseState.available = cp.vl["Cruise_Status"]['Cruise_State'] != 0
ret.gas = cp.vl["EngineData_14"]['ApedPosScal_Pc_Actl'] / 100.
ret.gasPressed = ret.gas > 1e-6
ret.brakePressed = bool(cp.vl["Cruise_Status"]["Brake_Drv_Appl"])
ret.brakeLights = bool(cp.vl["BCM_to_HS_Body"]["Brake_Lights"])
ret.genericToggle = bool(cp.vl["Steering_Buttons"]["Dist_Incr"])
# TODO: we also need raw driver torque, needed for Assisted Lane Change
self.lkas_state = cp.vl["Lane_Keep_Assist_Status"]['LaActAvail_D_Actl']
self.steer_error = cp.vl["Lane_Keep_Assist_Status"]['LaActDeny_B_Actl']
return ret
def get_d_path(self, v_ego, path_t, path_xyz):
# Reduce reliance on lanelines that are too far apart or
# will be in a few seconds
path_xyz[:,1] -= PATH_OFFSET
l_prob, r_prob = self.lll_prob, self.rll_prob
width_pts = self.rll_y - self.lll_y
prob_mods = []
for t_check in [0.0, 1.5, 3.0]:
width_at_t = interp(t_check * (v_ego + 7), self.ll_x, width_pts)
prob_mods.append(interp(width_at_t, [4.0, 5.0], [1.0, 0.0]))
mod = min(prob_mods)
l_prob *= mod
r_prob *= mod
# Reduce reliance on uncertain lanelines
l_std_mod = interp(self.lll_std, [.15, .3], [1.0, 0.0])
r_std_mod = interp(self.rll_std, [.15, .3], [1.0, 0.0])
l_prob *= l_std_mod
r_prob *= r_std_mod
if ENABLE_ZORROBYTE:
# zorrobyte code
if l_prob > 0.5 and r_prob > 0.5:
self.frame += 1
if self.frame > 20:
self.frame = 0
current_lane_width = clip(abs(self.rll_y[0] - self.lll_y[0]), 2.5, 3.5)
self.readings.append(current_lane_width)
self.lane_width = mean(self.readings)
if len(self.readings) >= 30:
self.readings.pop(0)
# zorrobyte
# Don't exit dive
if abs(self.rll_y[0] - self.lll_y[0]) > self.lane_width:
r_prob = r_prob / interp(l_prob, [0, 1], [1, 3])
else:
# Find current lanewidth
self.lane_width_certainty += 0.05 * (l_prob * r_prob - self.lane_width_certainty)
current_lane_width = abs(self.rll_y[0] - self.lll_y[0])
self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate)
speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5])
self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
(1 - self.lane_width_certainty) * speed_lane_width
clipped_lane_width = min(4.0, self.lane_width)
path_from_left_lane = self.lll_y + clipped_lane_width / 2.0
path_from_right_lane = self.rll_y - clipped_lane_width / 2.0
self.d_prob = l_prob + r_prob - l_prob * r_prob
# neokii
if ENABLE_INC_LANE_PROB and self.d_prob > 0.65:
self.d_prob = min(self.d_prob * 1.35, 1.0)
lane_path_y = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001)
lane_path_y_interp = np.interp(path_t, self.ll_t, lane_path_y)
path_xyz[:,1] = self.d_prob * lane_path_y_interp + (1.0 - self.d_prob) * path_xyz[:,1]
return path_xyz
def update(self, pt_cp, ch_cp):
ret = car.CarState.new_message()
self.prev_cruise_buttons = self.cruise_buttons
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]['ACCButtons']
ret.wheelSpeeds.fl = pt_cp.vl["EBCMWheelSpdFront"]['FLWheelSpd'] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = pt_cp.vl["EBCMWheelSpdFront"]['FRWheelSpd'] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = pt_cp.vl["EBCMWheelSpdRear"]['RLWheelSpd'] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = pt_cp.vl["EBCMWheelSpdRear"]['RRWheelSpd'] * CV.KPH_TO_MS
ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.01
ret.steeringAngle = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle']
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL"]['PRNDL'], None))
ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]['BrakePedalPosition'] / 0xd0
# Brake pedal's potentiometer returns near-zero reading even when pedal is not pressed.
if ret.brake < 10/0xd0:
ret.brake = 0.
ret.gas = pt_cp.vl["AcceleratorPedal"]['AcceleratorPedal'] / 254.
ret.gasPressed = ret.gas > 1e-5
ret.steeringTorque = pt_cp.vl["PSCMStatus"]['LKADriverAppldTrq']
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
# 1 - open, 0 - closed
ret.doorOpen = (pt_cp.vl["BCMDoorBeltStatus"]['FrontLeftDoor'] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]['FrontRightDoor'] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]['RearLeftDoor'] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]['RearRightDoor'] == 1)
# 1 - latched
ret.seatbeltUnlatched = pt_cp.vl["BCMDoorBeltStatus"]['LeftSeatBelt'] == 0
ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 1
ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 2
self.park_brake = pt_cp.vl["EPBStatus"]['EPBClosed']
ret.cruiseState.available = bool(pt_cp.vl["ECMEngineStatus"]['CruiseMainOn'])
ret.espDisabled = pt_cp.vl["ESPStatus"]['TractionControlOn'] != 1
self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]['CruiseState']
ret.brakePressed = ret.brake > 1e-5
# Regen braking is braking
if self.car_fingerprint == CAR.VOLT:
ret.brakePressed = ret.brakePressed or bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle'])
ret.cruiseState.enabled = self.pcm_acc_status != AccState.OFF
ret.cruiseState.standstill = self.pcm_acc_status == AccState.STANDSTILL
# 0 - inactive, 1 - active, 2 - temporary limited, 3 - failed
self.lkas_status = pt_cp.vl["PSCMStatus"]['LKATorqueDeliveredStatus']
ret.steerWarning = self.lkas_status not in [0, 1]
ret.brakeLights = ch_cp.vl["EBCMFrictionBrakeStatus"]["FrictionBrakePressure"] != 0 or ret.brakePressed
return ret
def get_fused_accel(self, apply_accel, stock_accel, sm):
dRel = 0.
lead = self.get_lead(sm)
if lead is not None:
dRel = lead.dRel
if stock_accel < apply_accel < -0.1:
stock_weight = interp(dRel, [2., 25.], [1., 0.])
apply_accel = apply_accel * (1. - stock_weight) + stock_accel * stock_weight
self.fused_decel.append(apply_accel)
if len(self.fused_decel) > 3:
self.fused_decel.pop(0)
return mean(self.fused_decel), dRel
def update(self, cp):
# 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
# calc best v_ego estimate, by averaging two opposite corners
self.v_wheel_fl = cp.vl["WheelSpeed_CG1"]['WhlRr_W_Meas'] * WHEEL_RADIUS
self.v_wheel_fr = cp.vl["WheelSpeed_CG1"]['WhlRl_W_Meas'] * WHEEL_RADIUS
self.v_wheel_rl = cp.vl["WheelSpeed_CG1"]['WhlFr_W_Meas'] * WHEEL_RADIUS
self.v_wheel_rr = cp.vl["WheelSpeed_CG1"]['WhlFl_W_Meas'] * WHEEL_RADIUS
v_wheel = mean([
self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr
])
# Kalman filter
if abs(
v_wheel - self.v_ego
) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.x = [[v_wheel], [0.0]]
self.v_ego_raw = v_wheel
v_ego_x = self.v_ego_kf.update(v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
self.standstill = not v_wheel > 0.001
self.angle_steers = cp.vl["Steering_Wheel_Data_CG1"][
'SteWhlRelInit_An_Sns']
self.v_cruise_pcm = cp.vl["Cruise_Status"]['Set_Speed'] * CV.MPH_TO_MS
self.pcm_acc_status = cp.vl["Cruise_Status"]['Cruise_State']
self.main_on = cp.vl["Cruise_Status"]['Cruise_State'] != 0
self.lkas_state = cp.vl["Lane_Keep_Assist_Status"]['LaActAvail_D_Actl']
# TODO: we also need raw driver torque, needed for Assisted Lane Change
self.steer_override = not cp.vl["Lane_Keep_Assist_Status"][
'LaHandsOff_B_Actl']
self.steer_error = cp.vl["Lane_Keep_Assist_Status"]['LaActDeny_B_Actl']
self.user_gas = cp.vl["EngineData_14"]['ApedPosScal_Pc_Actl']
self.brake_pressed = bool(cp.vl["Cruise_Status"]["Brake_Drv_Appl"])
self.brake_lights = bool(cp.vl["BCM_to_HS_Body"]["Brake_Lights"])
self.generic_toggle = bool(cp.vl["Steering_Buttons"]["Dist_Incr"])
def get_d_path(self, v_ego, path_t, path_xyz):
# Reduce reliance on lanelines that are too far apart or
# will be in a few seconds
path_xyz[:, 1] += self.path_offset
l_prob, r_prob = self.lll_prob, self.rll_prob
width_pts = self.rll_y - self.lll_y
prob_mods = []
for t_check in (0.0, 1.5, 3.0):
width_at_t = interp(t_check * (v_ego + 7), self.ll_x, width_pts)
prob_mods.append(interp(width_at_t, [4.0, 5.0], [1.0, 0.0]))
mod = min(prob_mods)
l_prob *= mod
r_prob *= mod
# Reduce reliance on uncertain lanelines
l_std_mod = interp(self.lll_std, [.15, .3], [1.0, 0.0])
r_std_mod = interp(self.rll_std, [.15, .3], [1.0, 0.0])
l_prob *= l_std_mod
r_prob *= r_std_mod
if ENABLE_ZORROBYTE:
# zorrobyte code
if l_prob > 0.5 and r_prob > 0.5:
self.frame += 1
if self.frame > 20:
self.frame = 0
current_lane_width = clip(abs(self.rll_y[0] - self.lll_y[0]), 2.5, 3.5)
self.readings.append(current_lane_width)
self.lane_width = mean(self.readings)
if len(self.readings) >= 30:
self.readings.pop(0)
# zorrobyte
# Don't exit dive
if abs(self.rll_y[0] - self.lll_y[0]) > self.lane_width:
r_prob = r_prob / interp(l_prob, [0, 1], [1, 3])
else:
# Find current lanewidth
self.lane_width_certainty.update(l_prob * r_prob)
current_lane_width = abs(self.rll_y[0] - self.lll_y[0])
self.lane_width_estimate.update(current_lane_width)
speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5])
self.lane_width = self.lane_width_certainty.x * self.lane_width_estimate.x + \
(1 - self.lane_width_certainty.x) * speed_lane_width
clipped_lane_width = min(4.0, self.lane_width)
path_from_left_lane = self.lll_y + clipped_lane_width / 2.0
path_from_right_lane = self.rll_y - clipped_lane_width / 2.0
self.d_prob = l_prob + r_prob - l_prob * r_prob
# neokii
if ENABLE_INC_LANE_PROB and self.d_prob > 0.65:
self.d_prob = min(self.d_prob * 1.3, 1.0)
lane_path_y = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001)
safe_idxs = np.isfinite(self.ll_t)
if safe_idxs[0]:
lane_path_y_interp = np.interp(path_t, self.ll_t[safe_idxs], lane_path_y[safe_idxs])
path_xyz[:,1] = self.d_prob * lane_path_y_interp + (1.0 - self.d_prob) * path_xyz[:,1]
else:
cloudlog.warning("Lateral mpc - NaNs in laneline times, ignoring")
return path_xyz
def update(self, cp, cp_cam):
ret = car.CarState.new_message()
ret.doorOpen = 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']])
ret.seatbeltUnlatched = cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED'] != 0
ret.brakePressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] != 0
ret.brakeLights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or ret.brakePressed)
if self.CP.enableGasInterceptor:
ret.gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
ret.gasPressed = ret.gas > 15
elif self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]:
ret.gas = cp.vl["GAS_PEDAL_ALT"]['GAS_PEDAL']
ret.gasPressed = ret.gas > 1e-5
else:
ret.gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
ret.gasPressed = cp.vl["PCM_CRUISE"]['GAS_RELEASED'] == 0
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS
ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.001
# Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use if non-zero
if abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']) > 1e-3:
self.accurate_steer_angle_seen = True
if self.accurate_steer_angle_seen:
if self.dp_toyota_zss:
ret.steeringAngle = cp.vl["SECONDARY_STEER_ANGLE"]['ZORRO_STEER'] - self.angle_offset
else:
ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
if self.needs_angle_offset:
angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
if (abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3) or self.dp_toyota_zss:
self.needs_angle_offset = False
self.angle_offset = ret.steeringAngle - angle_wheel
else:
ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
ret.steeringRate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
ret.leftBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
ret.rightBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']
ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_EPS']
# we could use the override bit from dbc, but it's triggered at too high torque values
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
ret.steerWarning = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5]
if self.CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_NXT]:
ret.cruiseState.available = cp.vl["DSU_CRUISE"]['MAIN_ON'] != 0
ret.cruiseState.speed = cp.vl["DSU_CRUISE"]['SET_SPEED'] * CV.KPH_TO_MS
self.low_speed_lockout = False
elif self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]:
ret.cruiseState.available = cp.vl["PCM_CRUISE_ALT"]['MAIN_ON'] != 0
ret.cruiseState.speed = cp.vl["PCM_CRUISE_ALT"]['SET_SPEED'] * CV.KPH_TO_MS
self.low_speed_lockout = False
else:
ret.cruiseState.available = cp.vl["PCM_CRUISE_2"]['MAIN_ON'] != 0
ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"]['SET_SPEED'] * CV.KPH_TO_MS
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2
if self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]:
# Lexus ISH does not have CRUISE_STATUS value (always 0), so we use CRUISE_ACTIVE value instead
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE']
else:
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor:
# ignore standstill in hybrid vehicles, since pcm allows to restart without
# receiving any special command. Also if interceptor is detected
ret.cruiseState.standstill = False
else:
ret.cruiseState.standstill = self.pcm_acc_status == 7
ret.cruiseState.enabled = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'])
# TODO: CRUISE_STATE is a 4 bit signal, find any other non-adaptive cruise states
ret.cruiseState.nonAdaptive = cp.vl["PCM_CRUISE"]['CRUISE_STATE'] in [5]
if self.CP.carFingerprint == CAR.PRIUS:
ret.genericToggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0
else:
ret.genericToggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM'])
ret.stockAeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5)
ret.espDisabled = cp.vl["ESP_CONTROL"]['TC_DISABLED'] != 0
# 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']
if self.CP.carFingerprint in TSS2_CAR:
ret.leftBlindspot = (cp.vl["BSM"]['L_ADJACENT'] == 1) or (cp.vl["BSM"]['L_APPROACHING'] == 1)
ret.rightBlindspot = (cp.vl["BSM"]['R_ADJACENT'] == 1) or (cp.vl["BSM"]['R_APPROACHING'] == 1)
return ret
sm = SubMaster(['deviceState', 'procLog'])
last_temp = 0.0
last_mem = 0.0
total_times = [0.] * 8
busy_times = [0.] * 8
prev_proclog: Optional[capnp._DynamicStructReader] = None
prev_proclog_t: Optional[int] = None
while True:
sm.update()
if sm.updated['deviceState']:
t = sm['deviceState']
last_temp = mean(t.cpuTempC)
last_mem = t.memoryUsagePercent
if sm.updated['procLog']:
m = sm['procLog']
cores = [0.] * 8
total_times_new = [0.] * 8
busy_times_new = [0.] * 8
for c in m.cpuTimes:
n = c.cpuNum
total_times_new[n] = cputime_total(c)
busy_times_new[n] = cputime_busy(c)
for n in range(8):
def update(self, cp, cp_cam):
# 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']
self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED']
if self.CP.enableGasInterceptor:
self.pedal_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] +
cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
else:
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
v_wheel = mean([
self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr
])
# Kalman filter
if abs(
v_wheel - self.v_ego
) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.x = [[v_wheel], [0.0]]
self.v_ego_raw = v_wheel
v_ego_x = self.v_ego_kf.update(v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
self.standstill = not v_wheel > 0.001
if self.CP.carFingerprint in TSS2_CAR:
self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']
elif self.CP.carFingerprint in NO_DSU_CAR:
# cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] is zeroed to where the steering angle is at start.
# need to apply an offset as soon as the steering angle measurements are both received
self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"][
'STEER_ANGLE'] - self.angle_offset
angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl[
"STEER_ANGLE_SENSOR"]['STEER_FRACTION']
if abs(angle_wheel) > 1e-3 and abs(
self.angle_steers) > 1e-3 and not self.init_angle_offset:
self.init_angle_offset = True
self.angle_offset = self.angle_steers - angle_wheel
else:
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']
can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
self.gear_shifter = parse_gear_shifter(can_gear, self.shifter_values)
try:
self.econ_on = cp.vl["GEAR_PACKET"]['ECON_ON']
except:
self.econ_on = 0
try:
self.sport_on = cp.vl["GEAR_PACKET"]['SPORT_ON']
except:
self.sport_on = 0
if self.sport_on == 1:
self.gasbuttonstatus = 1
if self.econ_on == 1:
self.gasbuttonstatus = 2
if self.sport_on == 0 and self.econ_on == 0:
self.gasbuttonstatus = 0
msg = arne182.Arne182Status.new_message()
msg.gasbuttonstatus = self.gasbuttonstatus
msg.readdistancelines = cp.vl["PCM_CRUISE_SM"]['DISTANCE_LINES']
if not travis:
self.arne182Status_sock.send(msg.to_bytes())
if self.CP.carFingerprint == CAR.LEXUS_IS:
self.main_on = cp.vl["DSU_CRUISE"]['MAIN_ON']
else:
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
# 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 [1, 5]
self.ipas_active = cp.vl['EPS_STATUS']['IPAS_STATE'] == 3
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']
# we could use the override bit from dbc, but it's triggered at too high torque values
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD
self.user_brake = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSURE']
if self.CP.carFingerprint == CAR.LEXUS_IS:
self.v_cruise_pcm = cp.vl["DSU_CRUISE"]['SET_SPEED']
self.low_speed_lockout = False
else:
self.v_cruise_pcm = cp.vl["PCM_CRUISE_2"]['SET_SPEED']
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"][
'LOW_SPEED_LOCKOUT'] == 2
if cp.vl["PCM_CRUISE"]['CRUISE_STATE'] and not self.pcm_acc_status:
if self.v_ego < 11.38:
self.setspeedoffset = max(
min(int(41.0 - self.v_ego * 3.6), 34.0), 0.0)
self.v_cruise_pcmlast = self.v_cruise_pcm
else:
self.setspeedoffset = 0
self.v_cruise_pcmlast = self.v_cruise_pcm
if self.v_cruise_pcm < self.v_cruise_pcmlast:
self.setspeedoffset = self.setspeedoffset + math.floor(
(int((-self.v_cruise_pcm) * 34 / 128 + 169 * 34 / 128) -
self.setspeedoffset) / (self.v_cruise_pcm - 40))
if self.v_cruise_pcmlast < self.v_cruise_pcm:
self.setspeedoffset = self.setspeedoffset + math.floor(
(int((-self.v_cruise_pcm) * 34 / 128 + 169 * 34 / 128) -
self.setspeedoffset) / (170 - self.v_cruise_pcm))
self.v_cruise_pcmlast = self.v_cruise_pcm
self.v_cruise_pcm = max(7,
int(self.v_cruise_pcm) - self.setspeedoffset)
if not self.left_blinker_on and not self.right_blinker_on:
self.Angles[self.Angle_counter] = abs(self.angle_steers)
self.v_cruise_pcm = int(
min(self.v_cruise_pcm,
interp(np.max(self.Angles), self.Angle, self.Angle_Speed)))
else:
self.Angles[self.Angle_counter] = 0
self.Angles_later[self.Angle_counter] = 0
self.Angle_counter = (self.Angle_counter + 1) % 250
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
self.pcm_acc_active = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'])
self.brake_lights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC']
or self.brake_pressed)
if self.CP.carFingerprint == CAR.PRIUS:
self.generic_toggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0
else:
self.generic_toggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM'])
self.tsgn1 = cp_cam.vl["RSA1"]['TSGN1']
self.spdval1 = cp_cam.vl["RSA1"]['SPDVAL1']
self.splsgn1 = cp_cam.vl["RSA1"]['SPLSGN1']
self.tsgn2 = cp_cam.vl["RSA1"]['TSGN2']
self.spdval2 = cp_cam.vl["RSA1"]['SPDVAL2']
self.splsgn2 = cp_cam.vl["RSA1"]['SPLSGN2']
self.tsgn3 = cp_cam.vl["RSA2"]['TSGN3']
self.splsgn3 = cp_cam.vl["RSA2"]['SPLSGN3']
self.tsgn4 = cp_cam.vl["RSA2"]['TSGN4']
self.splsgn4 = cp_cam.vl["RSA2"]['SPLSGN4']
self.noovertake = self.tsgn1 == 65 or self.tsgn2 == 65 or self.tsgn3 == 65 or self.tsgn4 == 65 or self.tsgn1 == 66 or self.tsgn2 == 66 or self.tsgn3 == 66 or self.tsgn4 == 66
if self.spdval1 > 0 or self.spdval2 > 0:
dat = arne182.LiveTrafficData.new_message()
if self.spdval1 > 0:
dat.speedLimitValid = True
if self.tsgn1 == 36:
dat.speedLimit = self.spdval1 * 1.60934
elif self.tsgn1 == 1:
dat.speedLimit = self.spdval1
else:
dat.speedLimit = 0
else:
dat.speedLimitValid = False
if self.spdval2 > 0:
dat.speedAdvisoryValid = True
dat.speedAdvisory = self.spdval2
else:
dat.speedAdvisoryValid = False
if not travis:
self.traffic_data_sock.send(dat.to_bytes())
def update(self, cp, cp_cam):
ret = car.CarState.new_message()
ret.doorOpen = 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"]
])
ret.seatbeltUnlatched = cp.vl["SEATS_DOORS"][
"SEATBELT_DRIVER_UNLATCHED"] != 0
ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0
if self.CP.enableGasInterceptor:
ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] +
cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2.
ret.gasPressed = ret.gas > 15
else:
ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL"]
ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"][
"WHEEL_SPEED_FL"] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"][
"WHEEL_SPEED_FR"] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"][
"WHEEL_SPEED_RL"] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"][
"WHEEL_SPEED_RR"] * CV.KPH_TO_MS
ret.vEgoRaw = mean([
ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl,
ret.wheelSpeeds.rr
])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.001
ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"][
"STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"]
torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]
# Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use if non-zero
if abs(torque_sensor_angle_deg) > 1e-3:
self.accurate_steer_angle_seen = True
if self.accurate_steer_angle_seen:
# Offset seems to be invalid for large steering angles
if abs(ret.steeringAngleDeg) < 90:
self.angle_offset.update(torque_sensor_angle_deg -
ret.steeringAngleDeg)
if self.angle_offset.initialized:
ret.steeringAngleOffsetDeg = self.angle_offset.x
ret.steeringAngleDeg = torque_sensor_angle_deg - self.angle_offset.x
ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_RATE"]
can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"])
ret.gearShifter = self.parse_gear_shifter(
self.shifter_values.get(can_gear, None))
ret.leftBlinker = cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 1
ret.rightBlinker = cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 2
ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"][
"STEER_TORQUE_DRIVER"]
ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"][
"STEER_TORQUE_EPS"]
# we could use the override bit from dbc, but it's triggered at too high torque values
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
ret.steerWarning = cp.vl["EPS_STATUS"]["LKA_STATE"] not in [1, 5]
if self.CP.carFingerprint == CAR.LEXUS_IS:
ret.cruiseState.available = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0
ret.cruiseState.speed = cp.vl["DSU_CRUISE"][
"SET_SPEED"] * CV.KPH_TO_MS
else:
ret.cruiseState.available = cp.vl["PCM_CRUISE_2"]["MAIN_ON"] != 0
ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"][
"SET_SPEED"] * CV.KPH_TO_MS
if self.CP.carFingerprint in TSS2_CAR:
self.acc_type = cp_cam.vl["ACC_CONTROL"]["ACC_TYPE"]
# some TSS2 cars have low speed lockout permanently set, so ignore on those cars
# these cars are identified by an ACC_TYPE value of 2.
# TODO: it may be possible to avoid the lockout and gain stop and go if you
# send your own ACC_CONTROL msg on startup with ACC_TYPE set to 1
if (self.CP.carFingerprint not in TSS2_CAR and self.CP.carFingerprint != CAR.LEXUS_IS) or \
(self.CP.carFingerprint in TSS2_CAR and self.acc_type == 1):
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"][
"LOW_SPEED_LOCKOUT"] == 2
self.pcm_acc_status = cp.vl["PCM_CRUISE"]["CRUISE_STATE"]
if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor:
# ignore standstill in hybrid vehicles, since pcm allows to restart without
# receiving any special command. Also if interceptor is detected
ret.cruiseState.standstill = False
else:
ret.cruiseState.standstill = self.pcm_acc_status == 7
ret.cruiseState.enabled = bool(cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"])
ret.cruiseState.nonAdaptive = cp.vl["PCM_CRUISE"]["CRUISE_STATE"] in [
1, 2, 3, 4, 5, 6
]
ret.genericToggle = bool(cp.vl["LIGHT_STALK"]["AUTO_HIGH_BEAM"])
ret.stockAeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"]
and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5)
ret.espDisabled = cp.vl["ESP_CONTROL"]["TC_DISABLED"] != 0
# 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"]
if self.CP.enableBsm:
ret.leftBlindspot = (cp.vl["BSM"]["L_ADJACENT"]
== 1) or (cp.vl["BSM"]["L_APPROACHING"] == 1)
ret.rightBlindspot = (cp.vl["BSM"]["R_ADJACENT"]
== 1) or (cp.vl["BSM"]["R_APPROACHING"] == 1)
self._update_traffic_signals(cp_cam)
ret.cruiseState.speedLimit = self._calculate_speed_limit()
return ret
def update(self, cp, cp_cam):
ret = car.CarState.new_message()
ret.doorOpen = 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']])
ret.seatbeltUnlatched = cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED'] != 0
ret.brakePressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] != 0
ret.brakeLights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or ret.brakePressed)
if self.CP.enableGasInterceptor:
ret.gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
ret.gasPressed = ret.gas > 15
else:
ret.gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
ret.gasPressed = ret.gas > 1e-5
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS
ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.001
if self.CP.carFingerprint in TSS2_CAR:
ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']
elif self.CP.carFingerprint in NO_DSU_CAR:
# cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] is zeroed to where the steering angle is at start.
# need to apply an offset as soon as the steering angle measurements are both received
ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
if abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3 and not self.init_angle_offset:
self.init_angle_offset = True
self.angle_offset = ret.steeringAngle - angle_wheel
else:
ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
ret.steeringRate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
ret.leftBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
ret.rightBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']
ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_EPS']
# we could use the override bit from dbc, but it's triggered at too high torque values
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
if self.CP.carFingerprint == CAR.LEXUS_IS:
ret.cruiseState.available = cp.vl["DSU_CRUISE"]['MAIN_ON'] != 0
ret.cruiseState.speed = cp.vl["DSU_CRUISE"]['SET_SPEED'] * CV.KPH_TO_MS
self.low_speed_lockout = False
else:
ret.cruiseState.available = cp.vl["PCM_CRUISE_2"]['MAIN_ON'] != 0
ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"]['SET_SPEED'] * CV.KPH_TO_MS
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor:
# ignore standstill in hybrid vehicles, since pcm allows to restart without
# receiving any special command. Also if interceptor is detected
ret.cruiseState.standstill = False
else:
ret.cruiseState.standstill = self.pcm_acc_status == 7
ret.cruiseState.enabled = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'])
if self.CP.carFingerprint == CAR.PRIUS:
ret.genericToggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0
else:
ret.genericToggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM'])
ret.stockAeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5)
ret.espDisabled = cp.vl["ESP_CONTROL"]['TC_DISABLED'] != 0
# 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_warning = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5]
self.ipas_active = cp.vl['EPS_STATUS']['IPAS_STATE'] == 3
return ret
def update(self, pt_cp):
ret = car.CarState.new_message()
self.prev_cruise_buttons = self.cruise_buttons
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]['ACCButtons']
ret.wheelSpeeds.fl = pt_cp.vl["EBCMWheelSpdFront"][
'FLWheelSpd'] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = pt_cp.vl["EBCMWheelSpdFront"][
'FRWheelSpd'] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = pt_cp.vl["EBCMWheelSpdRear"][
'RLWheelSpd'] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = pt_cp.vl["EBCMWheelSpdRear"][
'RRWheelSpd'] * CV.KPH_TO_MS
ret.vEgoRaw = mean([
ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl,
ret.wheelSpeeds.rr
])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.01
ret.steeringAngle = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle']
ret.gearShifter = self.parse_gear_shifter(
self.shifter_values.get(pt_cp.vl["ECMPRDNL"]['PRNDL'], None))
ret.brake = pt_cp.vl["EBCMBrakePedalPosition"][
'BrakePedalPosition'] / 0xd0
# Brake pedal's potentiometer returns near-zero reading even when pedal is not pressed.
if ret.brake < 10 / 0xd0:
ret.brake = 0.
# # TODO: need a better way to identify cars without ACC
# # TODO: this assumes the Pedal is present. If it isn't, this won't work...
# if self.CP.carFingerprint in (CAR.BOLT):
# ret.gas = self.pedal_gas / 256.
# else:
ret.gas = pt_cp.vl["AcceleratorPedal"]['AcceleratorPedal'] / 254.
# this is a hack for the interceptor. This is now only used in the simulation
# TODO: Replace tests by toyota so this can go away
if self.CP.enableGasInterceptor:
self.user_gas = (pt_cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] +
pt_cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
self.user_gas_pressed = self.user_gas > 20 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
ret.gasPressed = self.user_gas_pressed
#workaround for insta-disengage
#ret.gasPressed = False
else:
ret.gasPressed = ret.gas > 1e-5
#ret.gasPressed = ret.gas > 1e-5
ret.steeringTorque = pt_cp.vl["PSCMStatus"]['LKADriverAppldTrq']
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
# 1 - open, 0 - closed
ret.doorOpen = (pt_cp.vl["BCMDoorBeltStatus"]['FrontLeftDoor'] == 1
or pt_cp.vl["BCMDoorBeltStatus"]['FrontRightDoor'] == 1
or pt_cp.vl["BCMDoorBeltStatus"]['RearLeftDoor'] == 1
or pt_cp.vl["BCMDoorBeltStatus"]['RearRightDoor'] == 1)
# 1 - latched
ret.seatbeltUnlatched = pt_cp.vl["BCMDoorBeltStatus"][
'LeftSeatBelt'] == 0
ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 1
ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 2
if self.car_fingerprint in SUPERCRUISE_CARS:
self.park_brake = False
ret.cruiseState.available = False
ret.espDisabled = False
regen_pressed = False
self.pcm_acc_status = int(
pt_cp.vl["ASCMActiveCruiseControlStatus"]['ACCCmdActive'])
else:
self.park_brake = pt_cp.vl["EPBStatus"]['EPBClosed']
ret.cruiseState.available = bool(
pt_cp.vl["ECMEngineStatus"]['CruiseMainOn'])
ret.espDisabled = pt_cp.vl["ESPStatus"]['TractionControlOn'] != 1
self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]['CruiseState']
if self.car_fingerprint == CAR.VOLT or self.car_fingerprint == CAR.BOLT:
regen_pressed = bool(
pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle'])
else:
regen_pressed = False
# Regen braking is braking
ret.brakePressed = ret.brake > 1e-5 or regen_pressed
ret.cruiseState.enabled = self.pcm_acc_status != AccState.OFF
ret.cruiseState.standstill = self.pcm_acc_status == AccState.STANDSTILL
# 0 - inactive, 1 - active, 2 - temporary limited, 3 - failed
self.lkas_status = pt_cp.vl["PSCMStatus"]['LKATorqueDeliveredStatus']
self.steer_warning = not is_eps_status_ok(self.lkas_status,
self.car_fingerprint)
self.steer_not_allowed = not is_eps_status_ok(self.lkas_status,
self.car_fingerprint)
return ret
def dPath(self):
return mean([t.dPath for t in self.tracks])
def aRel(self):
return mean([t.aRel for t in self.tracks])
def vRel(self):
return mean([t.vRel for t in self.tracks])
def yRel(self):
return mean([t.yRel for t in self.tracks])
def dRel(self):
return mean([t.dRel for t in self.tracks])
def aLeadTau(self):
if all(t.cnt <= 1 for t in self.tracks):
return _LEAD_ACCEL_TAU
else:
return mean([t.aLeadTau for t in self.tracks if t.cnt > 1])
def update(self, cp, cp_cam):
ret = car.CarState.new_message()
ret.doorOpen = 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"]
])
ret.seatbeltUnlatched = cp.vl["SEATS_DOORS"][
"SEATBELT_DRIVER_UNLATCHED"] != 0
ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0
if self.CP.enableGasInterceptor:
ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] +
cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2.
ret.gasPressed = ret.gas > 15
else:
if self.CP.carFingerprint == CAR.LEXUS_ISH:
ret.gas = cp.vl["GAS_PEDAL_ALT"]['GAS_PEDAL']
else:
ret.gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"][
"WHEEL_SPEED_FL"] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"][
"WHEEL_SPEED_FR"] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"][
"WHEEL_SPEED_RL"] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"][
"WHEEL_SPEED_RR"] * CV.KPH_TO_MS
ret.vEgoRaw = mean([
ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl,
ret.wheelSpeeds.rr
])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.001
# Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use if non-zero
if self.dp_toyota_zss or abs(
cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]) > 1e-3:
self.accurate_steer_angle_seen = True
if self.accurate_steer_angle_seen:
if self.dp_toyota_zss:
ret.steeringAngleDeg = cp.vl["SECONDARY_STEER_ANGLE"][
"ZORRO_STEER"] - self.angle_offset
else:
ret.steeringAngleDeg = cp.vl["STEER_TORQUE_SENSOR"][
"STEER_ANGLE"] - self.angle_offset
if self.needs_angle_offset:
angle_wheel = cp.vl["STEER_ANGLE_SENSOR"][
"STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"][
"STEER_FRACTION"]
if abs(angle_wheel) > 1e-3:
self.needs_angle_offset = False
self.angle_offset = ret.steeringAngleDeg - angle_wheel
else:
ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"][
"STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"]
ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_RATE"]
can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"])
ret.gearShifter = self.parse_gear_shifter(
self.shifter_values.get(can_gear, None))
#dp: Thank you Arne (acceleration)
if self.dp_toyota_ap_btn_link:
if self.CP.carFingerprint in [
CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.CHRH,
CAR.PRIUS_TSS2, CAR.HIGHLANDERH_TSS2
]:
sport_on = cp.vl["GEAR_PACKET2"]['SPORT_ON']
econ_on = cp.vl["GEAR_PACKET2"]['ECON_ON']
else:
try:
econ_on = cp.vl["GEAR_PACKET"]['ECON_ON']
except KeyError:
econ_on = 0
if self.CP.carFingerprint == CAR.RAV4_TSS2:
sport_on = cp.vl["GEAR_PACKET"]['SPORT_ON_2']
else:
try:
sport_on = cp.vl["GEAR_PACKET"]['SPORT_ON']
except KeyError:
sport_on = 0
if sport_on == 0 and econ_on == 0:
self.dp_accel_profile = DP_ACCEL_NORMAL
elif sport_on == 1:
self.dp_accel_profile = DP_ACCEL_SPORT
elif econ_on == 1:
self.dp_accel_profile = DP_ACCEL_ECO
# if init is false, we sync profile with whatever mode we have on car
if not self.dp_accel_profile_init or self.dp_accel_profile != self.dp_accel_profile_prev:
put_nonblocking('dp_accel_profile', str(self.dp_accel_profile))
put_nonblocking('dp_last_modified', str(floor(time.time())))
self.dp_accel_profile_init = True
self.dp_accel_profile_prev = self.dp_accel_profile
#dp: Thank you Arne (distance button)
if self.dp_toyota_fp_btn_link:
if not self.read_distance_lines_init or self.read_distance_lines != cp.vl[
"PCM_CRUISE_SM"]['DISTANCE_LINES']:
self.read_distance_lines_init = True
self.read_distance_lines = cp.vl["PCM_CRUISE_SM"][
'DISTANCE_LINES']
put_nonblocking('dp_following_profile',
str(int(max(self.read_distance_lines, 0))))
put_nonblocking('dp_last_modified', str(floor(time.time())))
ret.leftBlinker = cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 1
ret.rightBlinker = cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 2
ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"][
"STEER_TORQUE_DRIVER"]
ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"][
"STEER_TORQUE_EPS"]
#dp
ret.engineRPM = cp.vl["ENGINE_RPM"]['RPM']
# we could use the override bit from dbc, but it's triggered at too high torque values
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
ret.steerWarning = cp.vl["EPS_STATUS"]["LKA_STATE"] not in [1, 5]
if self.CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_ISH]:
ret.cruiseState.available = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0
ret.cruiseState.speed = cp.vl["DSU_CRUISE"][
"SET_SPEED"] * CV.KPH_TO_MS
else:
ret.cruiseState.available = cp.vl["PCM_CRUISE_2"]["MAIN_ON"] != 0
ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"][
"SET_SPEED"] * CV.KPH_TO_MS
if self.CP.carFingerprint in TSS2_CAR:
self.acc_type = cp_cam.vl["ACC_CONTROL"]["ACC_TYPE"]
# some TSS2 cars have low speed lockout permanently set, so ignore on those cars
# these cars are identified by an ACC_TYPE value of 2.
# TODO: it may be possible to avoid the lockout and gain stop and go if you
# send your own ACC_CONTROL msg on startup with ACC_TYPE set to 1
if (self.CP.carFingerprint not in TSS2_CAR and self.CP.carFingerprint not in [CAR.LEXUS_IS, CAR.LEXUS_ISH]) or \
(self.CP.carFingerprint in TSS2_CAR and self.acc_type == 1):
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"][
"LOW_SPEED_LOCKOUT"] == 2
self.pcm_acc_status = cp.vl["PCM_CRUISE"]["CRUISE_STATE"]
if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor:
# ignore standstill in hybrid vehicles, since pcm allows to restart without
# receiving any special command. Also if interceptor is detected
ret.cruiseState.standstill = False
else:
ret.cruiseState.standstill = self.pcm_acc_status == 7
ret.cruiseState.enabled = bool(cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"])
ret.cruiseState.nonAdaptive = cp.vl["PCM_CRUISE"]["CRUISE_STATE"] in [
1, 2, 3, 4, 5, 6
]
ret.genericToggle = bool(cp.vl["LIGHT_STALK"]["AUTO_HIGH_BEAM"])
ret.stockAeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"]
and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5)
ret.espDisabled = cp.vl["ESP_CONTROL"]["TC_DISABLED"] != 0
# 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"]
#dp: distance button
self.distance = cp_cam.vl["ACC_CONTROL"]['DISTANCE']
if self.CP.enableBsm:
ret.leftBlindspot = (cp.vl["BSM"]["L_ADJACENT"]
== 1) or (cp.vl["BSM"]["L_APPROACHING"] == 1)
ret.rightBlindspot = (cp.vl["BSM"]["R_ADJACENT"]
== 1) or (cp.vl["BSM"]["R_APPROACHING"] == 1)
return ret
def _get_TR(self):
x_vel = [0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336] # velocities
if self.df_manager.is_auto: # decide which profile to use, model profile will be updated before this
df_profile = self.model_profile
else:
df_profile = self.user_profile
if df_profile != self.last_effective_profile:
self.profile_change_time = sec_since_boot()
self.last_effective_profile = df_profile
if df_profile == self.df_profiles.roadtrip:
y_dist = [1.5486, 1.556, 1.5655, 1.5773, 1.5964, 1.6246, 1.6715, 1.7057, 1.7859, 1.8542, 1.8697, 1.8833, 1.8961] # TRs
elif df_profile == self.df_profiles.traffic: # for in congested traffic
x_vel = [0.0, 1.892, 3.7432, 5.8632, 8.0727, 10.7301, 14.343, 17.6275, 22.4049, 28.6752, 34.8858, 40.35]
y_dist = [1.3781, 1.3791, 1.3457, 1.3134, 1.3145, 1.318, 1.3485, 1.257, 1.144, 0.979, 0.9461, 0.9156]
elif df_profile == self.df_profiles.relaxed: # default to relaxed/stock
y_dist = [1.385, 1.394, 1.406, 1.421, 1.444, 1.474, 1.521, 1.544, 1.568, 1.588, 1.599, 1.613, 1.634]
else:
raise Exception('Unknown profile type: {}'.format(df_profile))
# Global df mod
y_dist = self.global_profile_mod(x_vel, y_dist)
v_rel_dist_factor = self.idf_v_rel.integrate(self.lead_data.v_lead - self.car_data.v_ego)
a_lead_dist_factor = self.idf_a_lead.integrate(self.lead_data.a_lead) # TODO: should this be relative accel or just a_lead?
TR = interp(self.car_data.v_ego, x_vel, y_dist)
TR *= v_rel_dist_factor
TR *= a_lead_dist_factor
return TR
if self.car_data.v_ego > self.sng_speed: # keep sng distance until we're above sng speed again
self.sng = False
if (self.car_data.v_ego >= self.sng_speed or self.df_data.v_egos[0]['v_ego'] >= self.car_data.v_ego) and not self.sng:
# if above 15 mph OR we're decelerating to a stop, keep shorter TR. when we reaccelerate, use sng_TR and slowly decrease
TR = interp(self.car_data.v_ego, x_vel, y_dist)
else: # this allows us to get closer to the lead car when stopping, while being able to have smooth stop and go when reaccelerating
self.sng = True
x = [self.sng_speed * 0.7, self.sng_speed] # decrease TR between 12.6 and 18 mph from 1.8s to defined TR above at 18mph while accelerating
y = [self.sng_TR, interp(self.sng_speed, x_vel, y_dist)]
TR = interp(self.car_data.v_ego, x, y)
TR_mods = []
# Dynamic follow modifications (the secret sauce)
x = [-26, -15.6464, -9.8422, -6.0, -4.0, -2.68, -2.3, -1.8, -1.26, -0.61, 0, 0.61, 1.26, 2.1, 2.68, 4.4704] # relative velocity values
y = [1.76, 1.504, 1.34, 1.29, 1.25, 1.22, 1.19, 1.13, 1.053, 1.017, 1.0, 0.985, 0.958, 0.87, 0.81, 0.685] # multiplier values
y = np.array(y) - 1 # converts back to original abs mod
y *= 1.1 # multiplier for how much to mod
y = y / TR + 1 # converts back to multipliers
TR_mods.append(interp(self.lead_data.v_lead - self.car_data.v_ego, x, y))
x = [-4.4795, -2.8122, -1.5727, -1.1129, -0.6611, -0.2692, 0.0, 0.1466, 0.5144, 0.6903, 0.9302] # lead acceleration values
y = [1.16, 1.1067, 1.0613, 1.0343, 1.0203, 1.0147, 1.0, 0.9898, 0.972, 0.9647, 0.9607] # multiplier values
converted_with_TR = 1.5 # todo: do without numpy and simplify by converting with TR of 1, so only subtract
absolute_y_TR_mod = np.array(y) * converted_with_TR - converted_with_TR # converts back to original abs mod
absolute_y_TR_mod *= 1.2 # multiplier for how much to mod
y = absolute_y_TR_mod / TR + 1 # converts back to multipliers with accel mod of 1.4 taking current TR into account
TR_mods.append(interp(self.get_rel_accel(), x, y)) # todo: make this over more than 1 sec
# deadzone = self.car_data.v_ego / 3 # 10 mph at 30 mph # todo: tune pedal to react similarly to without before adding/testing this
# if self.lead_data.v_lead - deadzone > self.car_data.v_ego:
# TR_mods.append(self._relative_accel_mod())
# x = [self.sng_speed, self.sng_speed / 5.0] # as we approach 0, apply x% more distance
# y = [1.0, 1.05]
TR *= mean(TR_mods) # with mods as multipliers, profile mods shouldn't be needed
# if (self.car_data.left_blinker or self.car_data.right_blinker) and df_profile != self.df_profiles.traffic:
# x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph
# y = [1.0, .75, .65]
# TR *= interp(self.car_data.v_ego, x, y) # reduce TR when changing lanes
return float(clip(TR, self.min_TR, 2.7))
def update(self, cp, cp_cam, frame):
ret = car.CarState.new_message()
ret.doorOpen = 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']])
ret.seatbeltUnlatched = cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED'] != 0
ret.brakePressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] != 0
ret.brakeLights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or ret.brakePressed)
if self.CP.enableGasInterceptor:
ret.gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
ret.gasPressed = ret.gas > 15
else:
ret.gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
ret.gasPressed = cp.vl["PCM_CRUISE"]['GAS_RELEASED'] == 0
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS
ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.001
# Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use if non-zero
if abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']) > 1e-3:
self.accurate_steer_angle_seen = True
if self.accurate_steer_angle_seen:
ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
if self.needs_angle_offset:
angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
if abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3:
self.needs_angle_offset = False
self.angle_offset = ret.steeringAngle - angle_wheel
else:
ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
ret.steeringRate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
try:
self.econ_on = cp.vl["GEAR_PACKET"]['ECON_ON']
except:
self.econ_on = 0
if self.CP.carFingerprint in [CAR.COROLLAH_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_UXH_TSS2, CAR.CHRH]:
self.econ_on = cp.vl["GEAR_PACKET2"]['ECON_ON']
try:
self.sport_on = cp.vl["GEAR_PACKET"]['SPORT_ON']
except:
self.sport_on = 0
if self.CP.carFingerprint in [CAR.COROLLAH_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_UXH_TSS2, CAR.CHRH]:
self.sport_on = cp.vl["GEAR_PACKET2"]['SPORT_ON']
if self.sport_on == 1:
self.gasbuttonstatus = 1
if self.econ_on == 1:
self.gasbuttonstatus = 2
if self.sport_on == 0 and self.econ_on == 0:
self.gasbuttonstatus = 0
msg = messaging_arne.new_message('arne182Status')
if frame > 999 and not (self.CP.carFingerprint in TSS2_CAR):
if cp.vl["DEBUG"]['BLINDSPOTSIDE']==65: #Left
if cp.vl["DEBUG"]['BLINDSPOTD1'] != self.leftblindspotD1:
self.leftblindspotD1 = cp.vl["DEBUG"]['BLINDSPOTD1']
self.leftblindspotcounter = 21
if cp.vl["DEBUG"]['BLINDSPOTD2'] != self.leftblindspotD2:
self.leftblindspotD2 = cp.vl["DEBUG"]['BLINDSPOTD2']
self.leftblindspotcounter = 21
if (self.leftblindspotD1 > 10) or (self.leftblindspotD2 > 10):
self.leftblindspot = bool(1)
print("Left Blindspot Detected")
elif cp.vl["DEBUG"]['BLINDSPOTSIDE']==66: #Right
if cp.vl["DEBUG"]['BLINDSPOTD1'] != self.rightblindspotD1:
self.rightblindspotD1 = cp.vl["DEBUG"]['BLINDSPOTD1']
self.rightblindspotcounter = 21
if cp.vl["DEBUG"]['BLINDSPOTD2'] != self.rightblindspotD2:
self.rightblindspotD2 = cp.vl["DEBUG"]['BLINDSPOTD2']
self.rightblindspotcounter = 21
if (self.rightblindspotD1 > 10) or (self.rightblindspotD2 > 10):
self.rightblindspot = bool(1)
print("Right Blindspot Detected")
self.rightblindspotcounter = self.rightblindspotcounter -1 if self.rightblindspotcounter > 0 else 0
self.leftblindspotcounter = self.leftblindspotcounter -1 if self.leftblindspotcounter > 0 else 0
if self.leftblindspotcounter == 0:
self.leftblindspot = False
self.leftblindspotD1 = 0
self.leftblindspotD2 = 0
if self.rightblindspotcounter == 0:
self.rightblindspot = False
self.rightblindspotD1 = 0
self.rightblindspotD2 = 0
elif frame > 999 and self.CP.carFingerprint in TSS2_CAR:
self.leftblindspot = cp.vl["BSM"]['L_ADJACENT'] == 1
self.leftblindspotD1 = 10.1
self.leftblindspotD2 = 10.1
self.rightblindspot = cp.vl["BSM"]['R_ADJACENT'] == 1
self.rightblindspotD1 = 10.1
self.rightblindspotD2 = 10.1
msg.arne182Status.leftBlindspot = self.leftblindspot
ret.leftBlindspot = self.leftblindspot
msg.arne182Status.rightBlindspot = self.rightblindspot
ret.rightBlindspot = self.rightblindspot
msg.arne182Status.rightBlindspotD1 = self.rightblindspotD1
msg.arne182Status.rightBlindspotD2 = self.rightblindspotD2
msg.arne182Status.leftBlindspotD1 = self.leftblindspotD1
msg.arne182Status.leftBlindspotD2 = self.leftblindspotD2
msg.arne182Status.gasbuttonstatus = self.gasbuttonstatus
if not travis:
self.arne_sm.update(0)
self.sm.update(0)
self.smartspeed = self.sm['liveMapData'].speedLimit
self.arne_pm.send('arne182Status', msg)
self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
ret.leftBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
ret.rightBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']
ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_EPS']
# we could use the override bit from dbc, but it's triggered at too high torque values
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
ret.steerWarning = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5]
if self.CP.carFingerprint == CAR.LEXUS_IS:
self.main_on = cp.vl["DSU_CRUISE"]['MAIN_ON'] != 0
ret.cruiseState.speed = cp.vl["DSU_CRUISE"]['SET_SPEED']
self.low_speed_lockout = False
else:
self.main_on = cp.vl["PCM_CRUISE_2"]['MAIN_ON'] != 0
ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"]['SET_SPEED']
if self.CP.carFingerprint == CAR.COROLLAH_TSS2:
self.low_speed_lockout = False
else:
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2
ret.cruiseState.available = self.main_on
v_cruise_pcm_max = ret.cruiseState.speed
if self.CP.carFingerprint in TSS2_CAR:
minimum_set_speed = 27.0
elif self.CP.carFingerprint == CAR.RAV4:
minimum_set_speed = 44.0
else:
minimum_set_speed = 41.0
maximum_set_speed = 169.0
if self.CP.carFingerprint == CAR.LEXUS_RXH:
maximum_set_speed = 177.0
speed_range = maximum_set_speed-minimum_set_speed
if bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE']) and not self.pcm_acc_active and self.v_cruise_pcmlast != ret.cruiseState.speed:
if ret.vEgo < 12.5:
self.setspeedoffset = max(min(int(minimum_set_speed-ret.vEgo*3.6),(minimum_set_speed-7.0)),0.0)
self.v_cruise_pcmlast = ret.cruiseState.speed
if ret.cruiseState.speed < self.v_cruise_pcmlast:
if self.setspeedcounter > 0 and ret.cruiseState.speed > minimum_set_speed:
self.setspeedoffset = self.setspeedoffset + 4
else:
if math.floor((int((-ret.cruiseState.speed)*(minimum_set_speed-7.0)/speed_range + maximum_set_speed*(minimum_set_speed-7.0)/speed_range)-self.setspeedoffset)/(ret.cruiseState.speed-(minimum_set_speed-1.0))) > 0:
self.setspeedoffset = self.setspeedoffset + math.floor((int((-ret.cruiseState.speed)*(minimum_set_speed-7.0)/speed_range + maximum_set_speed*(minimum_set_speed-7.0)/speed_range)-self.setspeedoffset)/(ret.cruiseState.speed-(minimum_set_speed-1.0)))
self.setspeedcounter = 50
if self.v_cruise_pcmlast < ret.cruiseState.speed:
if self.setspeedcounter > 0 and (self.setspeedoffset - 4) > 0:
self.setspeedoffset = self.setspeedoffset - 4
else:
self.setspeedoffset = self.setspeedoffset + math.floor((int((-ret.cruiseState.speed)*(minimum_set_speed-7.0)/speed_range + maximum_set_speed*(minimum_set_speed-7.0)/speed_range)-self.setspeedoffset)/(maximum_set_speed+1.0-ret.cruiseState.speed))
self.setspeedcounter = 50
if self.setspeedcounter > 0:
self.setspeedcounter = self.setspeedcounter - 1
self.v_cruise_pcmlast = ret.cruiseState.speed
if int(ret.cruiseState.speed) - self.setspeedoffset < 7:
self.setspeedoffset = int(ret.cruiseState.speed) - 7
if int(ret.cruiseState.speed) - self.setspeedoffset > maximum_set_speed:
self.setspeedoffset = int(ret.cruiseState.speed) - maximum_set_speed
ret.cruiseState.speed = min(max(7, int(ret.cruiseState.speed) - self.setspeedoffset),v_cruise_pcm_max)
if not travis and self.arne_sm.updated['latControl'] and ret.vEgo > 11:
angle_later = self.arne_sm['latControl'].anglelater
else:
angle_later = 0
if not self.left_blinker_on and not self.right_blinker_on:
self.Angles[self.Angle_counter] = abs(ret.steeringAngle)
self.Angles_later[self.Angle_counter] = abs(angle_later)
else:
self.Angles[self.Angle_counter] = abs(ret.steeringAngle) * 0.8
if ret.vEgo > 11:
self.Angles_later[self.Angle_counter] = abs(angle_later) * 0.8
else:
self.Angles_later[self.Angle_counter] = 0.0
if self.gasbuttonstatus ==1:
factor = 1.6
elif self.gasbuttonstatus == 2:
factor = 1.0
else:
factor = 1.3
ret.cruiseState.speed = int(min(ret.cruiseState.speed, factor * interp(np.max(self.Angles), self.Angle, self.Angle_Speed)))
ret.cruiseState.speed = int(min(ret.cruiseState.speed, factor * interp(np.max(self.Angles_later), self.Angle, self.Angle_Speed)))
self.Angle_counter = (self.Angle_counter + 1 ) % 250
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor:
# ignore standstill in hybrid vehicles, since pcm allows to restart without
# receiving any special command. Also if interceptor is detected
ret.cruiseState.standstill = False
else:
ret.cruiseState.standstill = self.pcm_acc_status == 7
self.pcm_acc_active = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'])
ret.cruiseState.enabled = self.pcm_acc_active
if self.CP.carFingerprint in [CAR.PRIUS, CAR.PRIUS_2019]:
ret.genericToggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0
else:
ret.genericToggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM'])
ret.stockAeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5)
ret.espDisabled = cp.vl["ESP_CONTROL"]['TC_DISABLED'] != 0
# 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_warning = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5]
ret.cruiseState.speed = ret.cruiseState.speed * CV.KPH_TO_MS
#self.barriers = cp_cam.vl["LKAS_HUD"]['BARRIERS']
#self.rightline = cp_cam.vl["LKAS_HUD"]['RIGHT_LINE']
#self.leftline = cp_cam.vl["LKAS_HUD"]['LEFT_LINE']
self.tsgn1 = cp_cam.vl["RSA1"]['TSGN1']
if self.spdval1 != cp_cam.vl["RSA1"]['SPDVAL1']:
self.rsa_ignored_speed = 0
self.spdval1 = cp_cam.vl["RSA1"]['SPDVAL1']
self.splsgn1 = cp_cam.vl["RSA1"]['SPLSGN1']
self.tsgn2 = cp_cam.vl["RSA1"]['TSGN2']
#self.spdval2 = cp_cam.vl["RSA1"]['SPDVAL2']
self.splsgn2 = cp_cam.vl["RSA1"]['SPLSGN2']
self.tsgn3 = cp_cam.vl["RSA2"]['TSGN3']
self.splsgn3 = cp_cam.vl["RSA2"]['SPLSGN3']
self.tsgn4 = cp_cam.vl["RSA2"]['TSGN4']
self.splsgn4 = cp_cam.vl["RSA2"]['SPLSGN4']
self.noovertake = self.tsgn1 == 65 or self.tsgn2 == 65 or self.tsgn3 == 65 or self.tsgn4 == 65 or self.tsgn1 == 66 or self.tsgn2 == 66 or self.tsgn3 == 66 or self.tsgn4 == 66
if (self.spdval1 > 0) and not (self.spdval1 == 35 and self.tsgn1 == 1) and self.rsa_ignored_speed != self.spdval1:
dat = messaging_arne.new_message('liveTrafficData')
if self.spdval1 > 0:
dat.liveTrafficData.speedLimitValid = True
if self.tsgn1 == 36:
dat.liveTrafficData.speedLimit = self.spdval1 * 1.60934
elif self.tsgn1 == 1:
dat.liveTrafficData.speedLimit = self.spdval1
else:
dat.liveTrafficData.speedLimit = 0
else:
dat.liveTrafficData.speedLimitValid = False
#if self.spdval2 > 0:
# dat.liveTrafficData.speedAdvisoryValid = True
# dat.liveTrafficData.speedAdvisory = self.spdval2
#else:
dat.liveTrafficData.speedAdvisoryValid = False
if limit_rsa and rsa_max_speed < ret.vEgo:
dat.liveTrafficData.speedLimitValid = False
if not travis:
self.arne_pm.send('liveTrafficData', dat)
if ret.gasPressed and not self.gas_pressed:
self.engaged_when_gas_was_pressed = self.pcm_acc_active
if ((ret.gasPressed) or (self.gas_pressed and not ret.gasPressed)) and self.engaged_when_gas_was_pressed and ret.vEgo > self.smartspeed:
self.rsa_ignored_speed = self.spdval1
dat = messaging_arne.new_message('liveTrafficData')
dat.liveTrafficData.speedLimitValid = True
dat.liveTrafficData.speedLimit = ret.vEgo * 3.6
if not travis:
self.arne_pm.send('liveTrafficData', dat)
self.gas_pressed = ret.gasPressed
return ret
def vLeadK(self):
return mean([t.vLeadK for t in self.tracks])
def update(self, cp, cp_cam):
# 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']
self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED']
if self.CP.enableGasInterceptor:
self.pedal_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
else:
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
v_wheel = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr])
# Kalman filter
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.x = [[v_wheel], [0.0]]
self.v_ego_raw = v_wheel
v_ego_x = self.v_ego_kf.update(v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
self.standstill = not v_wheel > 0.001
if self.CP.carFingerprint in TSS2_CAR:
self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']
elif self.CP.carFingerprint in NO_DSU_CAR:
# cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] is zeroed to where the steering angle is at start.
# need to apply an offset as soon as the steering angle measurements are both received
self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
if abs(angle_wheel) > 1e-3 and abs(self.angle_steers) > 1e-3 and not self.init_angle_offset:
self.init_angle_offset = True
self.angle_offset = self.angle_steers - angle_wheel
else:
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']
can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
self.gear_shifter = parse_gear_shifter(can_gear, self.shifter_values)
if self.CP.carFingerprint == CAR.LEXUS_IS:
self.main_on = cp.vl["DSU_CRUISE"]['MAIN_ON']
else:
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
# 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 [1, 5]
self.ipas_active = cp.vl['EPS_STATUS']['IPAS_STATE'] == 3
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']
# we could use the override bit from dbc, but it's triggered at too high torque values
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD
self.user_brake = 0
if self.CP.carFingerprint == CAR.LEXUS_IS:
self.v_cruise_pcm = cp.vl["DSU_CRUISE"]['SET_SPEED']
self.low_speed_lockout = False
else:
self.v_cruise_pcm = cp.vl["PCM_CRUISE_2"]['SET_SPEED']
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
self.pcm_acc_active = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'])
self.brake_lights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or self.brake_pressed)
if self.CP.carFingerprint == CAR.PRIUS:
self.generic_toggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0
else:
self.generic_toggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM'])
self.stock_aeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5)
def aLeadK(self):
if all(t.cnt <= 1 for t in self.tracks):
return 0.
else:
return mean([t.aLeadK for t in self.tracks if t.cnt > 1])
def update(self, pt_cp):
self.prev_cruise_buttons = self.cruise_buttons
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]['ACCButtons']
self.prev_lka_button = self.lka_button
self.lka_button = pt_cp.vl["ASCMSteeringButton"]["LKAButton"]
self.prev_distance_button = self.distance_button
self.distance_button = pt_cp.vl["ASCMSteeringButton"]["DistanceButton"]
self.v_wheel_fl = pt_cp.vl["EBCMWheelSpdFront"]['FLWheelSpd'] * CV.KPH_TO_MS
self.v_wheel_fr = pt_cp.vl["EBCMWheelSpdFront"]['FRWheelSpd'] * CV.KPH_TO_MS
self.v_wheel_rl = pt_cp.vl["EBCMWheelSpdRear"]['RLWheelSpd'] * CV.KPH_TO_MS
self.v_wheel_rr = pt_cp.vl["EBCMWheelSpdRear"]['RRWheelSpd'] * CV.KPH_TO_MS
v_wheel = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr])
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.x = [[v_wheel], [0.0]]
self.v_ego_raw = v_wheel
v_ego_x = self.v_ego_kf.update(v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
self.standstill = self.v_ego_raw < 0.01
self.angle_steers = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle']
self.gear_shifter = parse_gear_shifter(pt_cp.vl["ECMPRDNL"]['PRNDL'])
self.user_brake = pt_cp.vl["EBCMBrakePedalPosition"]['BrakePedalPosition']
self.pedal_gas = pt_cp.vl["AcceleratorPedal"]['AcceleratorPedal']
self.user_gas_pressed = self.pedal_gas > 0
self.steer_torque_driver = pt_cp.vl["PSCMStatus"]['LKADriverAppldTrq']
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD
# 0 - inactive, 1 - active, 2 - temporary limited, 3 - failed
self.lkas_status = pt_cp.vl["PSCMStatus"]['LKATorqueDeliveredStatus']
self.steer_not_allowed = not is_eps_status_ok(self.lkas_status, self.car_fingerprint)
# 1 - open, 0 - closed
self.door_all_closed = (pt_cp.vl["BCMDoorBeltStatus"]['FrontLeftDoor'] == 0 and
pt_cp.vl["BCMDoorBeltStatus"]['FrontRightDoor'] == 0 and
pt_cp.vl["BCMDoorBeltStatus"]['RearLeftDoor'] == 0 and
pt_cp.vl["BCMDoorBeltStatus"]['RearRightDoor'] == 0)
# 1 - latched
self.seatbelt = pt_cp.vl["BCMDoorBeltStatus"]['LeftSeatBelt'] == 1
self.steer_error = False
self.brake_error = False
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
self.left_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 1
self.right_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 2
if self.car_fingerprint in SUPERCRUISE_CARS:
self.park_brake = False
self.main_on = False
self.acc_active = pt_cp.vl["ASCMActiveCruiseControlStatus"]['ACCCmdActive']
self.esp_disabled = False
self.regen_pressed = False
self.pcm_acc_status = int(self.acc_active)
else:
self.park_brake = pt_cp.vl["EPBStatus"]['EPBClosed']
self.main_on = pt_cp.vl["ECMEngineStatus"]['CruiseMainOn']
self.acc_active = False
self.esp_disabled = pt_cp.vl["ESPStatus"]['TractionControlOn'] != 1
self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]['CruiseState']
if self.car_fingerprint == CAR.VOLT:
self.regen_pressed = bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle'])
else:
self.regen_pressed = False
# Brake pedal's potentiometer returns near-zero reading
# even when pedal is not pressed.
if self.user_brake < 10:
self.user_brake = 0
# Regen braking is braking
self.brake_pressed = self.user_brake > 10 or self.regen_pressed
self.gear_shifter_valid = self.gear_shifter == car.CarState.GearShifter.drive
sm = SubMaster(['deviceState', 'procLog'])
last_temp = 0.0
last_mem = 0.0
total_times = [0.] * 8
busy_times = [0.] * 8
prev_proclog = None
prev_proclog_t = None
while True:
sm.update()
if sm.updated['deviceState']:
t = sm['deviceState']
last_temp = mean(t.cpuTempC)
last_mem = t.memoryUsagePercent
if sm.updated['procLog']:
m = sm['procLog']
cores = [0.] * 8
total_times_new = [0.] * 8
busy_times_new = [0.] * 8
for c in m.cpuTimes:
n = c.cpuNum
total_times_new[n] = cputime_total(c)
busy_times_new[n] = cputime_busy(c)
for n in range(8):
def vLat(self):
return mean([t.vLat for t in self.tracks])
def update(self, pt_cp):
ret = car.CarState.new_message()
self.prev_cruise_buttons = self.cruise_buttons
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]['ACCButtons']
self.prev_lka_button = self.lka_button
self.lka_button = pt_cp.vl["ASCMSteeringButton"]["LKAButton"]
self.prev_distance_button = self.distance_button
self.distance_button = pt_cp.vl["ASCMSteeringButton"]["DistanceButton"]
ret.wheelSpeeds.fl = pt_cp.vl["EBCMWheelSpdFront"]['FLWheelSpd'] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = pt_cp.vl["EBCMWheelSpdFront"]['FRWheelSpd'] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = pt_cp.vl["EBCMWheelSpdRear"]['RLWheelSpd'] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = pt_cp.vl["EBCMWheelSpdRear"]['RRWheelSpd'] * CV.KPH_TO_MS
ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.01
ret.steeringAngle = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle']
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL"]['PRNDL'], None))
ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]['BrakePedalPosition'] / 0xd0
# Brake pedal's potentiometer returns near-zero reading even when pedal is not pressed.
if ret.brake < 10/0xd0:
ret.brake = 0.
ret.gas = pt_cp.vl["AcceleratorPedal"]['AcceleratorPedal'] / 254.
ret.gasPressed = ret.gas > 1e-5
ret.steeringTorque = pt_cp.vl["PSCMStatus"]['LKADriverAppldTrq']
ret.steeringTorqueEps = pt_cp.vl["PSCMStatus"]['LKATotalTorqueDelivered']
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
# 1 - open, 0 - closed
ret.doorOpen = (pt_cp.vl["BCMDoorBeltStatus"]['FrontLeftDoor'] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]['FrontRightDoor'] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]['RearLeftDoor'] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]['RearRightDoor'] == 1)
# 1 - latched
ret.seatbeltUnlatched = pt_cp.vl["BCMDoorBeltStatus"]['LeftSeatBelt'] == 0
ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 1
ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 2
self.park_brake = pt_cp.vl["EPBStatus"]['EPBClosed']
self.main_on = bool(pt_cp.vl["ECMEngineStatus"]['CruiseMainOn'])
ret.espDisabled = pt_cp.vl["ESPStatus"]['TractionControlOn'] != 1
self.pcm_acc_status = pt_cp.vl["ASCMActiveCruiseControlStatus"]['ACCCmdActive']
ret.brakePressed = ret.brake > 1e-5
# Regen braking is braking
self.regen_pressed = False
if self.car_fingerprint == CAR.VOLT or self.car_fingerprint == CAR.BOLT:
self.regen_pressed = bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle'])
brake_light_enable = False
if self.car_fingerprint == CAR.BOLT:
if ret.aEgo < -1.3:
brake_light_enable = True
ret.brakeLights = ret.brakePressed or self.regen_pressed or brake_light_enable
ret.cruiseState.available = self.main_on
ret.cruiseState.enabled = self.pcm_acc_status != 0
ret.cruiseState.standstill = False
# 0 - inactive, 1 - active, 2 - temporary limited, 3 - failed
self.lkas_status = pt_cp.vl["PSCMStatus"]['LKATorqueDeliveredStatus']
ret.steerWarning = self.lkas_status not in [0, 1]
ret.steeringTorqueEps = pt_cp.vl["PSCMStatus"]['LKATorqueDelivered']
#self.engineRPM = pt_cp.vl["ECMEngineStatus"]['EngineRPM']
if self.car_fingerprint == CAR.BOLT:
self.HVBvoltage = pt_cp.vl["BECMBatteryVoltageCurrent"]['HVBatteryVoltage']
self.HVBcurrent = pt_cp.vl["BECMBatteryVoltageCurrent"]['HVBatteryCurrent']
# ret.hvBpower = self.HVBvoltage * self.HVBcurrent / 1000 #kW
return ret
