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