def fingerprint(logcan, sendcan, has_relay): params = Params() car_params = params.get("CarParams") if not travis: cached_fingerprint = params.get('CachedFingerprint') else: cached_fingerprint = None if car_params is not None: car_params = car.CarParams.from_bytes(car_params) if has_relay: # Vin query only reliably works thorugh OBDII bus = 1 cached_params = Params().get("CarParamsCache") if cached_params is not None: cached_params = car.CarParams.from_bytes(cached_params) if cached_params.carName == "mock": cached_params = None if cached_params is not None and len( cached_params.carFw ) > 0 and cached_params.carVin is not VIN_UNKNOWN: cloudlog.warning("Using cached CarParams") vin = cached_params.carVin car_fw = list(cached_params.carFw) else: cloudlog.warning("Getting VIN & FW versions") _, vin = get_vin(logcan, sendcan, bus) car_fw = get_fw_versions(logcan, sendcan, bus) fw_candidates = match_fw_to_car(car_fw) else: vin = VIN_UNKNOWN fw_candidates, car_fw = set(), [] cloudlog.warning("VIN %s", vin) Params().put("CarVin", vin) finger = gen_empty_fingerprint() candidate_cars = {i: all_known_cars() for i in [0, 1] } # attempt fingerprint on both bus 0 and 1 frame = 0 frame_fingerprint = 10 # 0.1s car_fingerprint = None done = False if cached_fingerprint is not None and use_car_caching: # if we previously identified a car and fingerprint and user hasn't disabled caching cached_fingerprint = json.loads(cached_fingerprint) if cached_fingerprint[0] is None or len(cached_fingerprint) < 3: params.delete('CachedFingerprint') else: finger[0] = { int(key): value for key, value in cached_fingerprint[2].items() } source = car.CarParams.FingerprintSource.can return (str(cached_fingerprint[0]), finger, vin, car_fw, cached_fingerprint[1]) while not done: a = messaging.get_one_can(logcan) for can in a.can: # need to independently try to fingerprint both bus 0 and 1 to work # for the combo black_panda and honda_bosch. Ignore extended messages # and VIN query response. # Include bus 2 for toyotas to disambiguate cars using camera messages # (ideally should be done for all cars but we can't for Honda Bosch) if can.src in range(0, 4): finger[can.src][can.address] = len(can.dat) for b in candidate_cars: if (can.src == b or (only_toyota_left(candidate_cars[b]) and can.src == 2)) and \ can.address < 0x800 and can.address not in [0x7df, 0x7e0, 0x7e8]: candidate_cars[b] = eliminate_incompatible_cars( can, candidate_cars[b]) # if we only have one car choice and the time since we got our first # message has elapsed, exit for b in candidate_cars: # Toyota needs higher time to fingerprint, since DSU does not broadcast immediately if only_toyota_left(candidate_cars[b]): frame_fingerprint = 100 # 1s if len(candidate_cars[b]) == 1: if frame > frame_fingerprint: # fingerprint done car_fingerprint = candidate_cars[b][0] elif len( candidate_cars[b] ) < 4: # For the RAV4 2019 and Corolla 2020 LE Fingerprint problem if frame > 180: if any(("TOYOTA COROLLA TSS2 2019" in c) for c in candidate_cars[b]): car_fingerprint = "TOYOTA COROLLA TSS2 2019" # bail if no cars left or we've been waiting for more than 2s failed = all(len(cc) == 0 for cc in candidate_cars.values()) or frame > 200 succeeded = car_fingerprint is not None done = failed or succeeded frame += 1 source = car.CarParams.FingerprintSource.can # If FW query returns exactly 1 candidate, use it if len(fw_candidates) == 1: car_fingerprint = list(fw_candidates)[0] source = car.CarParams.FingerprintSource.fw fixed_fingerprint = os.environ.get('FINGERPRINT', "") if len(fixed_fingerprint): car_fingerprint = fixed_fingerprint source = car.CarParams.FingerprintSource.fixed cloudlog.warning("fingerprinted %s", car_fingerprint) params.put( "CachedFingerprint", json.dumps([ car_fingerprint, source, {int(key): value for key, value in finger[0].items()} ])) return car_fingerprint, finger, vin, car_fw, source
def update_panda(): panda = None panda_dfu = None cloudlog.info("Connecting to panda") while True: # break on normal mode Panda panda_list = Panda.list() if len(panda_list) > 0: cloudlog.info("Panda found, connecting") panda = Panda(panda_list[0]) break # flash on DFU mode Panda panda_dfu = PandaDFU.list() if len(panda_dfu) > 0: cloudlog.info("Panda in DFU mode found, flashing recovery") panda_dfu = PandaDFU(panda_dfu[0]) panda_dfu.recover() time.sleep(1) fw_signature = get_expected_signature() try: serial = panda.get_serial()[0].decode("utf-8") except Exception: serial = None panda_version = "bootstub" if panda.bootstub else panda.get_version() panda_signature = b"" if panda.bootstub else panda.get_signature() cloudlog.warning( "Panda %s connected, version: %s, signature %s, expected %s" % ( serial, panda_version, panda_signature.hex(), fw_signature.hex(), )) if panda.bootstub or panda_signature != fw_signature: cloudlog.info("Panda firmware out of date, update required") panda.flash() cloudlog.info("Done flashing") if panda.bootstub: bootstub_version = panda.get_version() cloudlog.info( f"Flashed firmware not booting, flashing development bootloader. Bootstub version: {bootstub_version}" ) panda.recover() cloudlog.info("Done flashing bootloader") if panda.bootstub: cloudlog.info("Panda still not booting, exiting") raise AssertionError panda_signature = panda.get_signature() if panda_signature != fw_signature: cloudlog.info("Version mismatch after flashing, exiting") raise AssertionError
def update(self, sm, pm, CP, VM): v_ego = sm['carState'].vEgo angle_steers = sm['carState'].steeringAngle active = sm['controlsState'].active angle_offset = sm['liveParameters'].angleOffset lca_left = sm['carState'].lcaLeft lca_right = sm['carState'].lcaRight # Run MPC self.angle_steers_des_prev = self.angle_steers_des_mpc VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio) curvature_factor = VM.curvature_factor(v_ego) self.LP.parse_model(sm['model']) # Lane change logic one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN if sm['carState'].leftBlinker: self.lane_change_direction = LaneChangeDirection.left elif sm['carState'].rightBlinker: self.lane_change_direction = LaneChangeDirection.right if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or (not one_blinker) or (not self.lane_change_enabled): self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none else: if sm['carState'].leftBlinker: self.lane_change_direction = LaneChangeDirection.left elif sm['carState'].rightBlinker: self.lane_change_direction = LaneChangeDirection.right if self.lane_change_direction == LaneChangeDirection.left: torque_applied = sm['carState'].steeringTorque > 0 and sm['carState'].steeringPressed if CP.autoLcaEnabled and 1.5 > self.pre_auto_LCA_timer > 1.0 and not lca_left: torque_applied = True # Enable auto LCA only once after 2 sec else: torque_applied = sm['carState'].steeringTorque < 0 and sm['carState'].steeringPressed if CP.autoLcaEnabled and 1.5 > self.pre_auto_LCA_timer > 1.0 and not lca_right: torque_applied = True # Enable auto LCA only once after 2 sec lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob # State transitions # off if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: self.lane_change_state = LaneChangeState.preLaneChange # pre elif self.lane_change_state == LaneChangeState.preLaneChange: if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off elif torque_applied: if self.prev_torque_applied or self.lane_change_direction == LaneChangeDirection.left and not lca_left or \ self.lane_change_direction == LaneChangeDirection.right and not lca_right: self.lane_change_state = LaneChangeState.laneChangeStarting else: if self.pre_auto_LCA_timer < 10.: self.pre_auto_LCA_timer = 10. else: if self.pre_auto_LCA_timer > 10.3: self.prev_torque_applied = True # bsm elif self.lane_change_state == LaneChangeState.laneChangeStarting: if lca_left and self.lane_change_direction == LaneChangeDirection.left and not self.prev_torque_applied: self.lane_change_BSM = LaneChangeBSM.left self.lane_change_state = LaneChangeState.preLaneChange elif lca_right and self.lane_change_direction == LaneChangeDirection.right and not self.prev_torque_applied: self.lane_change_BSM = LaneChangeBSM.right self.lane_change_state = LaneChangeState.preLaneChange else: # starting self.lane_change_BSM = LaneChangeBSM.off if self.lane_change_state == LaneChangeState.laneChangeStarting and lane_change_prob > 0.5: self.lane_change_state = LaneChangeState.laneChangeFinishing # starting #elif self.lane_change_state == LaneChangeState.laneChangeStarting and lane_change_prob > 0.5: #self.lane_change_state = LaneChangeState.laneChangeFinishing # finishing elif self.lane_change_state == LaneChangeState.laneChangeFinishing and lane_change_prob < 0.2: if one_blinker: self.lane_change_state = LaneChangeState.preLaneChange else: self.lane_change_state = LaneChangeState.off if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]: self.lane_change_timer = 0.0 if self.lane_change_BSM == LaneChangeBSM.right: if not lca_right: self.lane_change_BSM = LaneChangeBSM.off if self.lane_change_BSM == LaneChangeBSM.left: if not lca_left: self.lane_change_BSM = LaneChangeBSM.off else: self.lane_change_timer += DT_MDL if self.lane_change_state == LaneChangeState.off: self.pre_auto_LCA_timer = 0.0 self.prev_torque_applied = False elif not (3. < self.pre_auto_LCA_timer < 10.): # stop afer 3 sec resume from 10 when torque applied self.pre_auto_LCA_timer += DT_MDL self.prev_one_blinker = one_blinker desire = DESIRES[self.lane_change_direction][self.lane_change_state] # Turn off lanes during lane change if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft: self.LP.l_prob = 0. self.LP.r_prob = 0. self.libmpc.init_weights(MPC_COST_LAT.PATH / 10.0, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost) else: self.libmpc.init_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost) self.LP.update_d_poly(v_ego) # account for actuation delay self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, CP.steerActuatorDelay) v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed self.libmpc.run_mpc(self.cur_state, self.mpc_solution, list(self.LP.l_poly), list(self.LP.r_poly), list(self.LP.d_poly), self.LP.l_prob, self.LP.r_prob, curvature_factor, v_ego_mpc, self.LP.lane_width) # reset to current steer angle if not active or overriding if active: delta_desired = self.mpc_solution[0].delta[1] rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR) else: delta_desired = math.radians(angle_steers - angle_offset) / VM.sR rate_desired = 0.0 self.cur_state[0].delta = delta_desired self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset) # Check for infeasable MPC solution mpc_nans = any(math.isnan(x) for x in self.mpc_solution[0].delta) t = sec_since_boot() if mpc_nans: self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost) self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning("Lateral mpc - nan: True") if self.mpc_solution[0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge self.solution_invalid_cnt += 1 else: self.solution_invalid_cnt = 0 plan_solution_valid = self.solution_invalid_cnt < 2 plan_send = messaging.new_message() plan_send.init('pathPlan') plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'model']) plan_send.pathPlan.laneWidth = float(self.LP.lane_width) plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly] plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly] plan_send.pathPlan.lProb = float(self.LP.l_prob) plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly] plan_send.pathPlan.rProb = float(self.LP.r_prob) plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc) plan_send.pathPlan.rateSteers = float(rate_desired) plan_send.pathPlan.angleOffset = float(sm['liveParameters'].angleOffsetAverage) plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid) plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid) plan_send.pathPlan.sensorValid = bool(sm['liveParameters'].sensorValid) plan_send.pathPlan.posenetValid = bool(sm['liveParameters'].posenetValid) plan_send.pathPlan.desire = desire plan_send.pathPlan.laneChangeState = self.lane_change_state plan_send.pathPlan.laneChangeDirection = self.lane_change_direction plan_send.pathPlan.laneChangeBSM = self.lane_change_BSM pm.send('pathPlan', plan_send) if LOG_MPC: dat = messaging.new_message() dat.init('liveMpc') dat.liveMpc.x = list(self.mpc_solution[0].x) dat.liveMpc.y = list(self.mpc_solution[0].y) dat.liveMpc.psi = list(self.mpc_solution[0].psi) dat.liveMpc.delta = list(self.mpc_solution[0].delta) dat.liveMpc.cost = self.mpc_solution[0].cost pm.send('liveMpc', dat)
def uninstall(): cloudlog.warning("uninstalling") with open('/cache/recovery/command', 'w') as f: f.write('--wipe_data\n') # IPowerManager.reboot(confirm=false, reason="recovery", wait=true) os.system("service call power 16 i32 0 s16 recovery i32 1")
def update(self, sm, pm, CP, VM): v_ego = sm['carState'].vEgo stand_still = sm['carState'].standStill angle_steers = sm['carState'].steeringAngle active = sm['controlsState'].active anglesteer_current = sm['controlsState'].angleSteers anglesteer_desire = sm['controlsState'].angleSteersDes #live_steerratio = sm['liveParameters'].steerRatio mode_select = sm['carState'].cruiseState.modeSel lateral_control_method = sm['controlsState'].lateralControlMethod if lateral_control_method == 0: output_scale = sm['controlsState'].lateralControlState.pidState.output elif lateral_control_method == 1: output_scale = sm['controlsState'].lateralControlState.indiState.output elif lateral_control_method == 2: output_scale = sm['controlsState'].lateralControlState.lqrState.output angle_offset = sm['liveParameters'].angleOffset live_steer_ratio = sm['liveParameters'].steerRatio if live_steer_ratio != CP.steerRatio: self.steerRatio_range = [CP.steerRatio, live_steer_ratio] # Run MPC self.angle_steers_des_prev = self.angle_steers_des_mpc self.angle_diff = abs(anglesteer_desire) - abs(anglesteer_current) if abs(output_scale) >= 1 and v_ego > 8: self.new_steerRatio_prev = interp(self.angle_diff, self.angle_differ_range, self.steerRatio_range) if self.new_steerRatio_prev > self.new_steerRatio: self.new_steerRatio = self.new_steerRatio_prev #self.new_steer_rate_cost = interp(self.angle_diff, self.angle_differ_range, self.steer_rate_cost_range) #if abs(output_scale) >= 1 and v_ego > 8 and ((abs(anglesteer_desire) - abs(anglesteer_current)) > 20): # self.mpc_frame += 1 # if self.mpc_frame % 10 == 0: # self.new_steerRatio += (round(v_ego, 1) * 0.025) # if live_steer_ratio != CP.steerRatio: # if self.new_steerRatio >= live_steer_ratio: # self.new_steerRatio = live_steer_ratio # else: # if self.new_steerRatio >= 17.5: # self.new_steerRatio = 17.5 # self.mpc_frame = 0 else: self.mpc_frame += 1 if self.mpc_frame % 10 == 0: self.new_steerRatio -= 0.1 if self.new_steerRatio <= CP.steerRatio: self.new_steerRatio = CP.steerRatio #self.new_steer_rate_cost += 0.02 #if self.new_steer_rate_cost >= CP.steerRateCost: # self.new_steer_rate_cost = CP.steerRateCost self.mpc_frame = 0 self.new_steer_actuator_delay = interp(v_ego, self.steer_actuator_delay_vel, self.steer_actuator_delay_range) # Update vehicle model x = max(sm['liveParameters'].stiffnessFactor, 0.1) #sr = max(sm['liveParameters'].steerRatio, 0.1) sr = max(self.new_steerRatio, 0.1) VM.update_params(x, sr) curvature_factor = VM.curvature_factor(v_ego) self.LP.parse_model(sm['model']) # Lane change logic one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN if sm['carState'].leftBlinker: self.lane_change_direction = LaneChangeDirection.left elif sm['carState'].rightBlinker: self.lane_change_direction = LaneChangeDirection.right #if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or (not one_blinker) or (not self.lane_change_enabled): if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or (not self.lane_change_enabled) or ( abs(output_scale) >= 0.9 and self.lane_change_timer > 1): self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none else: torque_applied = sm['carState'].steeringPressed and \ ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or (sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) blindspot_detected = ((sm['carState'].leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or (sm['carState'].rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob # State transitions # off if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: self.lane_change_state = LaneChangeState.preLaneChange self.lane_change_ll_prob = 1.0 self.lane_change_wait_timer = 0 # pre elif self.lane_change_state == LaneChangeState.preLaneChange: self.lane_change_wait_timer += DT_MDL if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off elif not blindspot_detected and (torque_applied or (self.lane_change_auto_delay and self.lane_change_wait_timer > self.lane_change_auto_delay)): self.lane_change_state = LaneChangeState.laneChangeStarting # starting elif self.lane_change_state == LaneChangeState.laneChangeStarting: # fade out over .5s self.lane_change_adjust_new = interp(v_ego, self.lane_change_adjust_vel, self.lane_change_adjust) self.lane_change_ll_prob = max(self.lane_change_ll_prob - self.lane_change_adjust_new*DT_MDL, 0.0) # 98% certainty if lane_change_prob < 0.03 and self.lane_change_ll_prob < 0.02: self.lane_change_state = LaneChangeState.laneChangeFinishing # finishing elif self.lane_change_state == LaneChangeState.laneChangeFinishing: # fade in laneline over 1s self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0) if one_blinker and self.lane_change_ll_prob > 0.98: self.lane_change_state = LaneChangeState.preLaneChange elif self.lane_change_ll_prob > 0.98: self.lane_change_state = LaneChangeState.off if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]: self.lane_change_timer = 0.0 else: self.lane_change_timer += DT_MDL self.prev_one_blinker = one_blinker desire = DESIRES[self.lane_change_direction][self.lane_change_state] # Turn off lanes during lane change if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft: self.LP.l_prob *= self.lane_change_ll_prob self.LP.r_prob *= self.lane_change_ll_prob self.LP.update_d_poly(v_ego, sm) # account for actuation delay self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, self.new_steer_actuator_delay) v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed self.libmpc.run_mpc(self.cur_state, self.mpc_solution, list(self.LP.l_poly), list(self.LP.r_poly), list(self.LP.d_poly), self.LP.l_prob, self.LP.r_prob, curvature_factor, v_ego_mpc, self.LP.lane_width) # reset to current steer angle if not active or overriding if active: delta_desired = self.mpc_solution[0].delta[1] rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR) else: delta_desired = math.radians(angle_steers - angle_offset) / VM.sR rate_desired = 0.0 self.cur_state[0].delta = delta_desired self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset) # Check for infeasable MPC solution mpc_nans = any(math.isnan(x) for x in self.mpc_solution[0].delta) t = sec_since_boot() if mpc_nans: self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.new_steer_rate_cost) self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning("Lateral mpc - nan: True") if self.mpc_solution[0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge self.solution_invalid_cnt += 1 else: self.solution_invalid_cnt = 0 plan_solution_valid = self.solution_invalid_cnt < 3 plan_send = messaging.new_message('pathPlan') plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'model']) plan_send.pathPlan.laneWidth = float(self.LP.lane_width) plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly] plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly] plan_send.pathPlan.lProb = float(self.LP.l_prob) plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly] plan_send.pathPlan.rProb = float(self.LP.r_prob) plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc) plan_send.pathPlan.rateSteers = float(rate_desired) if self.angle_offset_select == 0: plan_send.pathPlan.angleOffset = float(sm['liveParameters'].angleOffsetAverage) else: plan_send.pathPlan.angleOffset = float(sm['liveParameters'].angleOffset) plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid) plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid) plan_send.pathPlan.desire = desire plan_send.pathPlan.laneChangeState = self.lane_change_state plan_send.pathPlan.laneChangeDirection = self.lane_change_direction plan_send.pathPlan.steerRatio = VM.sR plan_send.pathPlan.steerActuatorDelay = self.new_steer_actuator_delay plan_send.pathPlan.steerRateCost = self.new_steer_rate_cost plan_send.pathPlan.outputScale = output_scale if stand_still: self.standstill_elapsed_time += DT_MDL else: self.standstill_elapsed_time = 0.0 plan_send.pathPlan.standstillElapsedTime = int(self.standstill_elapsed_time) pm.send('pathPlan', plan_send) if LOG_MPC: dat = messaging.new_message('liveMpc') dat.liveMpc.x = list(self.mpc_solution[0].x) dat.liveMpc.y = list(self.mpc_solution[0].y) dat.liveMpc.psi = list(self.mpc_solution[0].psi) dat.liveMpc.delta = list(self.mpc_solution[0].delta) dat.liveMpc.cost = self.mpc_solution[0].cost pm.send('liveMpc', dat)
def fingerprint(logcan, sendcan, has_relay): fixed_fingerprint = os.environ.get('FINGERPRINT', "") skip_fw_query = os.environ.get('SKIP_FW_QUERY', False) if has_relay and not fixed_fingerprint and not skip_fw_query: # Vin query only reliably works thorugh OBDII bus = 1 cached_params = Params().get("CarParamsCache") if cached_params is not None: cached_params = car.CarParams.from_bytes(cached_params) if cached_params.carName == "mock": cached_params = None if cached_params is not None and len(cached_params.carFw) > 0 and cached_params.carVin is not VIN_UNKNOWN: cloudlog.warning("Using cached CarParams") vin = cached_params.carVin car_fw = list(cached_params.carFw) else: cloudlog.warning("Getting VIN & FW versions") _, vin = get_vin(logcan, sendcan, bus) car_fw = get_fw_versions(logcan, sendcan, bus) exact_fw_match, fw_candidates = match_fw_to_car(car_fw) else: vin = VIN_UNKNOWN exact_fw_match, fw_candidates, car_fw = True, set(), [] cloudlog.warning("VIN %s", vin) Params().put("CarVin", vin) finger = gen_empty_fingerprint() candidate_cars = {i: all_known_cars() for i in [0]} # attempt fingerprint on bus 0 only frame = 0 frame_fingerprint = 10 # 0.1s car_fingerprint = None done = False while not done: a = get_one_can(logcan) for can in a.can: # need to independently try to fingerprint both bus 0 and 1 to work # for the combo black_panda and honda_bosch. Ignore extended messages # and VIN query response. # Include bus 2 for toyotas to disambiguate cars using camera messages # (ideally should be done for all cars but we can't for Honda Bosch) if can.src in range(0, 4): finger[can.src][can.address] = len(can.dat) for b in candidate_cars: if (can.src == b or (only_toyota_left(candidate_cars[b]) and can.src == 2)) and \ can.address < 0x800 and can.address not in [0x7df, 0x7e0, 0x7e8]: candidate_cars[b] = eliminate_incompatible_cars(can, candidate_cars[b]) # if we only have one car choice and the time since we got our first # message has elapsed, exit for b in candidate_cars: # Toyota needs higher time to fingerprint, since DSU does not broadcast immediately if only_toyota_left(candidate_cars[b]): frame_fingerprint = 100 # 1s if len(candidate_cars[b]) == 1 and frame > frame_fingerprint: # fingerprint done car_fingerprint = candidate_cars[b][0] # bail if no cars left or we've been waiting for more than 2s failed = (all(len(cc) == 0 for cc in candidate_cars.values()) and frame > frame_fingerprint) or frame > 200 succeeded = car_fingerprint is not None done = failed or succeeded frame += 1 exact_match = True source = car.CarParams.FingerprintSource.can # If FW query returns exactly 1 candidate, use it if len(fw_candidates) == 1: car_fingerprint = list(fw_candidates)[0] source = car.CarParams.FingerprintSource.fw exact_match = exact_fw_match if fixed_fingerprint: car_fingerprint = fixed_fingerprint source = car.CarParams.FingerprintSource.fixed cloudlog.warning("fingerprinted %s", car_fingerprint) return car_fingerprint, finger, vin, car_fw, source, exact_match
def update(self, sm): v_ego = sm['carState'].vEgo active = sm['controlsState'].active measured_curvature = sm['controlsState'].curvature md = sm['modelV2'] self.LP.parse_model(sm['modelV2']) if len(md.position.x) == TRAJECTORY_SIZE and len(md.orientation.x) == TRAJECTORY_SIZE: self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z]) self.t_idxs = np.array(md.position.t) self.plan_yaw = list(md.orientation.z) if len(md.position.xStd) == TRAJECTORY_SIZE: self.path_xyz_stds = np.column_stack([md.position.xStd, md.position.yStd, md.position.zStd]) # Lane change logic one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or (not one_blinker) or (not self.lane_change_enabled): self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none else: torque_applied = sm['carState'].steeringPressed and \ ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or (sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) or \ self.auto_lane_change_enabled and \ (AUTO_LCA_START_TIME+0.25) > self.auto_lane_change_timer > AUTO_LCA_START_TIME blindspot_detected = ((sm['carState'].leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or (sm['carState'].rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob # State transitions # off if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: if sm['carState'].leftBlinker: self.lane_change_direction = LaneChangeDirection.left elif sm['carState'].rightBlinker: self.lane_change_direction = LaneChangeDirection.right self.lane_change_state = LaneChangeState.preLaneChange self.lane_change_ll_prob = 1.0 # pre elif self.lane_change_state == LaneChangeState.preLaneChange: if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off elif torque_applied and (not blindspot_detected or self.prev_torque_applied): self.lane_change_state = LaneChangeState.laneChangeStarting elif torque_applied and blindspot_detected and self.auto_lane_change_timer != 10.0: self.auto_lane_change_timer = 10.0 elif not torque_applied and self.auto_lane_change_timer == 10.0 and not self.prev_torque_applied: self.prev_torque_applied = True # LaneChangeState.laneChangeStarting elif self.lane_change_state == LaneChangeState.laneChangeStarting: # fade out over .5s self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2*DT_MDL, 0.0) # 98% certainty if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01: self.lane_change_state = LaneChangeState.laneChangeFinishing # LaneChangeState.laneChangeFinishing elif self.lane_change_state == LaneChangeState.laneChangeFinishing: # fade in laneline over 1s self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0) if self.lane_change_ll_prob > 0.99: self.lane_change_direction = LaneChangeDirection.none if one_blinker: self.lane_change_state = LaneChangeState.preLaneChange else: self.lane_change_state = LaneChangeState.off if self.lane_change_state in (LaneChangeState.off, LaneChangeState.preLaneChange): self.lane_change_timer = 0.0 else: self.lane_change_timer += DT_MDL if self.lane_change_state == LaneChangeState.off: self.auto_lane_change_timer = 0.0 self.prev_torque_applied = False elif self.auto_lane_change_timer < (AUTO_LCA_START_TIME+0.25): # stop afer 3 sec resume from 10 when torque applied self.auto_lane_change_timer += DT_MDL self.prev_one_blinker = one_blinker self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] # Send keep pulse once per second during LaneChangeStart.preLaneChange if self.lane_change_state in (LaneChangeState.off, LaneChangeState.laneChangeStarting): self.keep_pulse_timer = 0.0 elif self.lane_change_state == LaneChangeState.preLaneChange: self.keep_pulse_timer += DT_MDL if self.keep_pulse_timer > 1.0: self.keep_pulse_timer = 0.0 elif self.desire in (log.LateralPlan.Desire.keepLeft, log.LateralPlan.Desire.keepRight): self.desire = log.LateralPlan.Desire.none # Turn off lanes during lane change if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft: self.LP.lll_prob *= self.lane_change_ll_prob self.LP.rll_prob *= self.lane_change_ll_prob if self.use_lanelines: d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz) self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, ntune_common_get('steerRateCost')) else: d_path_xyz = self.path_xyz path_cost = np.clip(abs(self.path_xyz[0, 1] / self.path_xyz_stds[0, 1]), 0.5, 1.5) * MPC_COST_LAT.PATH # Heading cost is useful at low speed, otherwise end of plan can be off-heading heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.0]) self.lat_mpc.set_weights(path_cost, heading_cost, ntune_common_get('steerRateCost')) y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:,1]) heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw) self.y_pts = y_pts assert len(y_pts) == LAT_MPC_N + 1 assert len(heading_pts) == LAT_MPC_N + 1 # self.x0[4] = v_ego p = np.array([v_ego, CAR_ROTATION_RADIUS]) self.lat_mpc.run(self.x0, p, y_pts, heading_pts) # init state for next self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3]) # Check for infeasible MPC solution mpc_nans = np.isnan(self.lat_mpc.x_sol[:, 3]).any() t = sec_since_boot() if mpc_nans or self.lat_mpc.solution_status != 0: self.reset_mpc() self.x0[3] = measured_curvature if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning("Lateral mpc - nan: True") if self.lat_mpc.cost > 20000. or mpc_nans: self.solution_invalid_cnt += 1 else: self.solution_invalid_cnt = 0
def update(self, sm, pm, CP, VM): v_ego = sm['carState'].vEgo angle_steers = sm['carState'].steeringAngle active = sm['controlsState'].active angle_offset = sm['liveParameters'].angleOffset self.LP.update(v_ego, sm['model']) # Run MPC self.angle_steers_des_prev = self.angle_steers_des_mpc VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio) curvature_factor = VM.curvature_factor(v_ego) # TODO: Check for active, override, and saturation # if active: # self.path_offset_i += self.LP.d_poly[3] / (60.0 * 20.0) # self.path_offset_i = clip(self.path_offset_i, -0.5, 0.5) # self.LP.d_poly[3] += self.path_offset_i # else: # self.path_offset_i = 0.0 # account for actuation delay self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, CP.steerActuatorDelay) v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed self.libmpc.run_mpc(self.cur_state, self.mpc_solution, list(self.LP.l_poly), list(self.LP.r_poly), list(self.LP.d_poly), self.LP.l_prob, self.LP.r_prob, curvature_factor, v_ego_mpc, self.LP.lane_width) # reset to current steer angle if not active or overriding if active: delta_desired = self.mpc_solution[0].delta[1] rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR) else: delta_desired = math.radians(angle_steers - angle_offset) / VM.sR rate_desired = 0.0 self.cur_state[0].delta = delta_desired self.angle_steers_des_mpc = float( math.degrees(delta_desired * VM.sR) + angle_offset) # Check for infeasable MPC solution mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta))) t = sec_since_boot() if mpc_nans: self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost) self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning("Lateral mpc - nan: True") if self.mpc_solution[ 0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge self.solution_invalid_cnt += 1 else: self.solution_invalid_cnt = 0 plan_solution_valid = self.solution_invalid_cnt < 2 plan_send = messaging.new_message() plan_send.init('pathPlan') plan_send.valid = sm.all_alive_and_valid(service_list=[ 'carState', 'controlsState', 'liveParameters', 'model' ]) plan_send.pathPlan.laneWidth = float(self.LP.lane_width) plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly] plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly] plan_send.pathPlan.lProb = float(self.LP.l_prob) plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly] plan_send.pathPlan.rProb = float(self.LP.r_prob) plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc) plan_send.pathPlan.rateSteers = float(rate_desired) plan_send.pathPlan.angleOffset = float( sm['liveParameters'].angleOffsetAverage) plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid) plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid) plan_send.pathPlan.sensorValid = bool(sm['liveParameters'].sensorValid) plan_send.pathPlan.posenetValid = bool( sm['liveParameters'].posenetValid) pm.send('pathPlan', plan_send) if LOG_MPC: dat = messaging.new_message() dat.init('liveMpc') dat.liveMpc.x = list(self.mpc_solution[0].x) dat.liveMpc.y = list(self.mpc_solution[0].y) dat.liveMpc.psi = list(self.mpc_solution[0].psi) dat.liveMpc.delta = list(self.mpc_solution[0].delta) dat.liveMpc.cost = self.mpc_solution[0].cost pm.send('liveMpc', dat)
def update(self, pm, CS, lead): v_ego = CS.vEgo # Setup current mpc state self.cur_state[0].x_ego = 0.0 if lead is not None and lead.status: self.x_lead = x_lead = lead.dRel self.v_lead = v_lead = max(0.0, lead.vLead) a_lead = lead.aLeadK if (v_lead < 0.1 or -a_lead / 2.0 > v_lead): v_lead = 0.0 a_lead = 0.0 self.a_lead_tau = lead.aLeadTau self.new_lead = False if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5: self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead, a_lead, self.a_lead_tau) self.new_lead = True self.prev_lead_status = True self.prev_lead_x = x_lead self.cur_state[0].x_l = x_lead self.cur_state[0].v_l = v_lead else: self.prev_lead_status = False # Fake a fast lead car, so mpc keeps running self.cur_state[0].x_l = 50.0 self.cur_state[0].v_l = v_ego + 10.0 a_lead = 0.0 self.a_lead_tau = _LEAD_ACCEL_TAU self.v_lead = 0. self.x_lead = 0. # Calculate mpc t = sec_since_boot() maxTR = interp(v_ego, BpTr, TrY) if v_ego < 25. or self.x_lead < 25.: maxTR = max(maxTR, interp((self.v_lead - v_ego), BpvlTr, TrvlY)) if self.v_lead < v_ego + .6 and v_ego > .3: if self.x_lead < 25.: maxTR *= 1.1 TR = self.last_TR + .01 elif self.v_lead - v_ego < -10.: maxTR *= 0.75 TR = self.last_TR + .005 else: TR = self.last_TR + .005 else: if self.x_lead > 65.: maxTR *= 0.85 TR = self.last_TR - .025 else: TR = self.last_TR - .0025 TR = clip(TR, 0.7, maxTR) if v_ego < 5. and self.v_lead > v_ego + .5: TR = 0.25 TR = clip(TR, 0.25, maxTR) self.last_TR = TR n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, TR) duration = int((sec_since_boot() - t) * 1e9) if LOG_MPC: self.send_mpc_solution(pm, n_its, duration) # Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed self.v_mpc = self.mpc_solution[0].v_ego[1] self.a_mpc = self.mpc_solution[0].a_ego[1] self.v_mpc_future = self.mpc_solution[0].v_ego[10] # Reset if NaN or goes through lead car crashing = any(lead - ego < -50 for ( lead, ego) in zip(self.mpc_solution[0].x_l, self.mpc_solution[0].x_ego)) nans = any(math.isnan(x) for x in self.mpc_solution[0].v_ego) backwards = min(self.mpc_solution[0].v_ego) < -0.01 if ((backwards or crashing) and self.prev_lead_status) or nans: if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning( "Longitudinal mpc %d reset - backwards: %s crashing: %s nan: %s" % (self.mpc_id, backwards, crashing, nans)) self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) self.cur_state[0].v_ego = v_ego self.cur_state[0].a_ego = 0.0 self.v_mpc = v_ego self.a_mpc = CS.aEgo self.prev_lead_status = False
def update(self, sm, pm, CP, VM): limit_steers1 = 0 limit_steers2 = 0 debug_status = 0 v_ego = sm['carState'].vEgo angle_steers = sm['carState'].steeringAngle active = sm['controlsState'].active steeringPressed = sm['carState'].steeringPressed saturated_steering = sm['controlsState'].steerSaturated live_steerratio = sm['liveParameters'].steerRatio mode_select = sm['carState'].cruiseState.modeSel steeringTorque = sm['carState'].steeringTorque angle_offset = sm['liveParameters'].angleOffset model_sum = sm['controlsState'].modelSum v_ego_kph = v_ego * CV.MS_TO_KPH # Run MPC self.angle_steers_des_prev = self.angle_steers_des_mpc if saturated_steering: self.mpc_frame += 1 if self.mpc_frame % 10 == 0: self.new_steerRatio += 0.1 if self.new_steerRatio >= 16.5: self.new_steerRatio = 16.5 self.mpc_frame = 0 else: self.mpc_frame += 1 if self.mpc_frame % 20 == 0: self.new_steerRatio -= 0.1 if self.new_steerRatio <= CP.steerRatio: self.new_steerRatio = CP.steerRatio self.mpc_frame = 0 # Update vehicle model x = max(sm['liveParameters'].stiffnessFactor, 0.1) #sr = max(sm['liveParameters'].steerRatio, 0.1) sr = max(self.new_steerRatio, 0.1) VM.update_params(x, sr) curvature_factor = VM.curvature_factor(v_ego) self.LP.parse_model(sm['model']) # Lane change logic one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN if sm['carState'].leftBlinker: self.lane_change_direction = LaneChangeDirection.left elif sm['carState'].rightBlinker: self.lane_change_direction = LaneChangeDirection.right if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or (not one_blinker) or (not self.lane_change_enabled): self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none else: torque_applied = sm['carState'].steeringPressed and \ ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or (sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) blindspot_detected = ((sm['carState'].leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or (sm['carState'].rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob # State transitions # off if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: self.lane_change_state = LaneChangeState.preLaneChange self.lane_change_ll_prob = 1.0 self.lane_change_wait_timer = 0 # pre elif self.lane_change_state == LaneChangeState.preLaneChange: self.lane_change_wait_timer += DT_MDL if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off elif not blindspot_detected and (torque_applied or (self.lane_change_auto_delay and self.lane_change_wait_timer > self.lane_change_auto_delay)): self.lane_change_state = LaneChangeState.laneChangeStarting # starting elif self.lane_change_state == LaneChangeState.laneChangeStarting: # fade out over .5s self.lane_change_adjust_new = interp(v_ego, self.lane_change_adjust_vel, self.lane_change_adjust) self.lane_change_ll_prob = max(self.lane_change_ll_prob - self.lane_change_adjust_new*DT_MDL, 0.0) # 98% certainty if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01: self.lane_change_state = LaneChangeState.laneChangeFinishing # finishing elif self.lane_change_state == LaneChangeState.laneChangeFinishing: # fade in laneline over 1s self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0) if one_blinker and self.lane_change_ll_prob > 0.99: self.lane_change_state = LaneChangeState.laneChangeDone # done elif self.lane_change_state == LaneChangeState.laneChangeDone: if not one_blinker: self.lane_change_state = LaneChangeState.off if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]: self.lane_change_timer = 0.0 else: self.lane_change_timer += DT_MDL self.prev_one_blinker = one_blinker desire = DESIRES[self.lane_change_direction][self.lane_change_state] # Turn off lanes during lane change if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft: self.LP.l_prob *= self.lane_change_ll_prob self.LP.r_prob *= self.lane_change_ll_prob self.LP.update_d_poly(v_ego, sm) # account for actuation delay if mode_select == 3: self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, (CP.steerActuatorDelay + 0.05)) else: self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, CP.steerActuatorDelay) v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed self.libmpc.run_mpc(self.cur_state, self.mpc_solution, list(self.LP.l_poly), list(self.LP.r_poly), list(self.LP.d_poly), self.LP.l_prob, self.LP.r_prob, curvature_factor, v_ego_mpc, self.LP.lane_width) # reset to current steer angle if not active or overriding if active: delta_desired = self.mpc_solution[0].delta[1] rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR) else: delta_desired = math.radians(angle_steers - angle_offset) / VM.sR rate_desired = 0.0 self.cur_state[0].delta = delta_desired self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset) # Atom # org_angle_steers_des = self.angle_steers_des_mpc # delta_steer = org_angle_steers_des - angle_steers # if self.lane_change_state == LaneChangeState.laneChangeStarting: # debug_status = 0 # xp = [40,70] # fp2 = [5,10] # limit_steers = interp( v_ego_kph, xp, fp2 ) # self.angle_steers_des_mpc = self.limit_ctrl( org_angle_steers_des, limit_steers, angle_steers ) # elif steeringPressed: # debug_status = 1 # if angle_steers > 10 and steeringTorque > 0: # delta_steer = max( delta_steer, 0 ) # delta_steer = min( delta_steer, DST_ANGLE_LIMIT ) # self.angle_steers_des_mpc = angle_steers + delta_steer # elif angle_steers < -10 and steeringTorque < 0: # delta_steer = min( delta_steer, 0 ) # delta_steer = max( delta_steer, -DST_ANGLE_LIMIT ) # self.angle_steers_des_mpc = angle_steers + delta_steer # else: # debug_status = 2 # if steeringTorque < 0: # right # if delta_steer > 0: # self.angle_steers_des_mpc = self.limit_ctrl( org_angle_steers_des, DST_ANGLE_LIMIT, angle_steers ) # elif steeringTorque > 0: # left # if delta_steer < 0: # self.angle_steers_des_mpc = self.limit_ctrl( org_angle_steers_des, DST_ANGLE_LIMIT, angle_steers ) # elif v_ego_kph < 20 : #and abs(angle_steers) < 5.0 : # debug_status = 3 # # 저속 와리가리 제어. # xp = [5,10,20] # fp2 = [1,3,7] # limit_steers = interp( v_ego_kph, xp, fp2 ) # self.angle_steers_des_mpc = self.limit_ctrl( org_angle_steers_des, limit_steers, angle_steers ) # elif v_ego_kph > 85: # debug_status = 4 # pass # elif abs(angle_steers) > 25: # # #최대 허용 조향각 제어 로직 1. # debug_status = 5 # xp = [-30,-20,-10,-5,0,5,10,20,30] # 5=>약12도, 10=>28 15=>35, 30=>52 # fp1 = [ 3, 5, 7, 9,11,13,15,13,11,9] # + # fp2 = [ 9,11,13,15,13,11, 9, 7, 5,3] # - # # fp1 = [ 5, 7, 9,11,13,15,17,15,12] # + # # fp2 = [12,15,17,15,13,11, 9, 7, 5] # - # # xp = [-40,-30,-20,-10,-5,0,5,10,20,30,40] # 5=>약12도, 10=>28 15=>35, 30=>52 # # fp1 = [ 3, 5, 7, 9,11,13,15,17,15,12,10] # + # # fp2 = [10,12,15,17,15,13,11, 9, 7, 5, 3] # - # limit_steers1 = interp( model_sum, xp, fp1 ) # + # limit_steers2 = interp( model_sum, xp, fp2 ) # - # self.angle_steers_des_mpc = self.limit_ctrl1( org_angle_steers_des, limit_steers1, limit_steers2, angle_steers ) # str1 = '#/{} CVs/{} LS1/{} LS2/{} Ang/{} oDES/{} delta1/{} fDES/{} '.format( # debug_status, model_sum, limit_steers1, limit_steers2, angle_steers, org_angle_steers_des, delta_steer, self.angle_steers_des_mpc) # # Conan : 최대 허용 조향각 제어 로직 2. # delta_steer2 = self.angle_steers_des_mpc - angle_steers # if delta_steer2 > DST_ANGLE_LIMIT: # p_angle_steers = angle_steers + DST_ANGLE_LIMIT # self.angle_steers_des_mpc = p_angle_steers # elif delta_steer2 < -DST_ANGLE_LIMIT: # m_angle_steers = angle_steers - DST_ANGLE_LIMIT # self.angle_steers_des_mpc = m_angle_steers # str2 = 'delta2/{} fDES2/{}'.format( # delta_steer2, self.angle_steers_des_mpc) # self.trRapidCurv.add( str1 + str2 ) # Hoya : 가변 sR rate_cost # self.sr_boost_bp = [ 10.0, 15.0, 20.0, 30.0] # self.sR_Cost = [ 1.00, 0.75, 0.60, 0.30] # steerRateCost = interp(abs(angle_steers), self.sr_boost_bp, self.sR_Cost) # Check for infeasable MPC solution mpc_nans = any(math.isnan(x) for x in self.mpc_solution[0].delta) t = sec_since_boot() if mpc_nans: self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost) # CP.steerRateCost < steerRateCost self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning("Lateral mpc - nan: True") if self.mpc_solution[0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge self.solution_invalid_cnt += 1 else: self.solution_invalid_cnt = 0 plan_solution_valid = self.solution_invalid_cnt < 3 plan_send = messaging.new_message('pathPlan') plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'model']) plan_send.pathPlan.laneWidth = float(self.LP.lane_width) plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly] plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly] plan_send.pathPlan.lProb = float(self.LP.l_prob) plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly] plan_send.pathPlan.rProb = float(self.LP.r_prob) plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc) plan_send.pathPlan.rateSteers = float(rate_desired) plan_send.pathPlan.angleOffset = float(sm['liveParameters'].angleOffset) #plan_send.pathPlan.angleOffset = float(sm['liveParameters'].angleOffsetAverage) plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid) plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid) plan_send.pathPlan.desire = desire plan_send.pathPlan.laneChangeState = self.lane_change_state plan_send.pathPlan.laneChangeDirection = self.lane_change_direction plan_send.pathPlan.steerRatio = VM.sR plan_send.pathPlan.steerActuatorDelay = CP.steerActuatorDelay pm.send('pathPlan', plan_send) if LOG_MPC: dat = messaging.new_message('liveMpc') dat.liveMpc.x = list(self.mpc_solution[0].x) dat.liveMpc.y = list(self.mpc_solution[0].y) dat.liveMpc.psi = list(self.mpc_solution[0].psi) dat.liveMpc.delta = list(self.mpc_solution[0].delta) dat.liveMpc.cost = self.mpc_solution[0].cost pm.send('liveMpc', dat)
def update_panda(): repo_version = get_expected_version() panda = None panda_dfu = None cloudlog.info("Connecting to panda") while True: # break on normal mode Panda panda_list = Panda.list() if len(panda_list) > 0: cloudlog.info("Panda found, connecting") panda = Panda(panda_list[0]) break # flash on DFU mode Panda panda_dfu = PandaDFU.list() if len(panda_dfu) > 0: cloudlog.info("Panda in DFU mode found, flashing recovery") panda_dfu = PandaDFU(panda_dfu[0]) panda_dfu.recover() print("waiting for board...") time.sleep(1) try: serial = panda.get_serial()[0].decode("utf-8") except Exception: serial = None current_version = "bootstub" if panda.bootstub else panda.get_version() cloudlog.warning("Panda %s connected, version: %s, expected %s" % (serial, current_version, repo_version)) if panda.bootstub or not current_version.startswith(repo_version): cloudlog.info("Panda firmware out of date, update required") signed_fn = os.path.join(BASEDIR, "board", "obj", "panda.bin.signed") if os.path.exists(signed_fn): cloudlog.info("Flashing signed firmware") panda.flash(fn=signed_fn) else: cloudlog.info("Building and flashing unsigned firmware") panda.flash() cloudlog.info("Done flashing") if panda.bootstub: cloudlog.info( "Flashed firmware not booting, flashing development bootloader") panda.recover() cloudlog.info("Done flashing bootloader") if panda.bootstub: cloudlog.info("Panda still not booting, exiting") raise AssertionError version = panda.get_version() if not version.startswith(repo_version): cloudlog.info("Version mismatch after flashing, exiting") raise AssertionError
def fingerprint(logcan, timeout): if os.getenv("SIMULATOR2") is not None: return ("simulator2", None) elif os.getenv("SIMULATOR") is not None: return ("simulator", None) params = Params() cached_fingerprint = params.get('CachedFingerprint') if cached_fingerprint is not None and kegman.get( "useCarCaching", True ): # if we previously identified a car and fingerprint and user hasn't disabled caching cached_fingerprint = json.loads(cached_fingerprint) try: with open("/data/kegman.json", "r") as f: cloudlog.warning(str(f.read())) except: pass try: with open("/data/params/d/ControlsParams", "r") as f: cloudlog.warning(f.read()) except: pass try: with open("/data/params/d/LiveParameters", "r") as f: cloudlog.warning(f.read()) except: pass return (str(cached_fingerprint[0]), { long(key): value for key, value in cached_fingerprint[1].items() }) # not sure if dict of longs is required cloudlog.warning("waiting for fingerprint...") candidate_cars = all_known_cars() finger = {} st = None st_passive = sec_since_boot() # only relevant when passive can_seen = False while 1: for a in messaging.drain_sock(logcan): for can in a.can: can_seen = True # ignore everything not on bus 0 and with more than 11 bits, # which are ussually sporadic and hard to include in fingerprints if can.src == 0 and can.address < 0x800: finger[can.address] = len(can.dat) candidate_cars = eliminate_incompatible_cars( can, candidate_cars) if st is None and can_seen: st = sec_since_boot() # start time ts = sec_since_boot() # if we only have one car choice and the time_fingerprint since we got our first # message has elapsed, exit. Toyota needs higher time_fingerprint, since DSU does not # broadcast immediately if len(candidate_cars) == 1 and st is not None: # TODO: better way to decide to wait more if Toyota time_fingerprint = 1.0 if ("TOYOTA" in candidate_cars[0] or "LEXUS" in candidate_cars[0]) else 0.1 if (ts - st) > time_fingerprint: break # bail if no cars left or we've been waiting too long elif len(candidate_cars) == 0 or (timeout and (ts - st_passive) > timeout): return None, finger time.sleep(0.01) try: with open("/data/kegman.json", "r") as f: cloudlog.warning(str(f.read())) except: pass try: with open("/data/params/d/ControlsParams", "r") as f: cloudlog.warning(f.read()) except: pass try: with open("/data/params/d/LiveParameters", "r") as f: cloudlog.warning(f.read()) except: pass cloudlog.warning("fingerprinted %s", candidate_cars[0]) params.put("CachedFingerprint", json.dumps([ candidate_cars[0], {int(key): value for key, value in finger.items()} ])) # probably can remove long to int conversion return (candidate_cars[0], finger)
def get_fw_versions(logcan, sendcan, bus, extra=None, timeout=0.1, debug=False, progress=False): ecu_types = {} # Extract ECU adresses to query from fingerprints # ECUs using a subadress need be queried one by one, the rest can be done in parallel addrs = [] parallel_addrs = [] versions = get_attr_from_cars('FW_VERSIONS', combine_brands=False) if extra is not None: versions.update(extra) for brand, brand_versions in versions.items(): for c in brand_versions.values(): for ecu_type, addr, sub_addr in c.keys(): a = (brand, addr, sub_addr) if a not in ecu_types: ecu_types[(addr, sub_addr)] = ecu_type if sub_addr is None: if a not in parallel_addrs: parallel_addrs.append(a) else: if [a] not in addrs: addrs.append([a]) addrs.insert(0, parallel_addrs) fw_versions = {} for i, addr in enumerate(tqdm(addrs, disable=not progress)): for addr_chunk in chunks(addr): for brand, request, response in REQUESTS: try: addrs = [(a, s) for (b, a, s) in addr_chunk if b in (brand, 'any')] if addrs: query = IsoTpParallelQuery(sendcan, logcan, bus, addrs, request, response, debug=debug) t = 2 * timeout if i == 0 else timeout fw_versions.update(query.get_data(t)) except Exception: cloudlog.warning( f"FW query exception: {traceback.format_exc()}") # Build capnp list to put into CarParams car_fw = [] for addr, version in fw_versions.items(): f = car.CarParams.CarFw.new_message() f.ecu = ecu_types[addr] f.fwVersion = version f.address = addr[0] if addr[1] is not None: f.subAddress = addr[1] car_fw.append(f) return car_fw
def update(self, CP, VM, CS, md, live100): v_ego = CS.carState.vEgo angle_steers = CS.carState.steeringAngle active = live100.live100.active angle_offset = live100.live100.angleOffset self.MP.update(v_ego, md) # Run MPC self.angle_steers_des_prev = self.angle_steers_des_mpc curvature_factor = VM.curvature_factor(v_ego) l_poly = libmpc_py.ffi.new("double[4]", list(self.MP.l_poly)) r_poly = libmpc_py.ffi.new("double[4]", list(self.MP.r_poly)) p_poly = libmpc_py.ffi.new("double[4]", list(self.MP.p_poly)) # account for actuation delay #self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, CP.steerRatio, CP.steerActuatorDelay) v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed self.libmpc.run_mpc(self.cur_state, self.mpc_solution, l_poly, r_poly, p_poly, self.MP.l_prob, self.MP.r_prob, self.MP.p_prob, curvature_factor, v_ego_mpc, self.MP.lane_width) # reset to current steer angle if not active or overriding if active: delta_desired = self.mpc_solution[0].delta[1] else: delta_desired = math.radians(angle_steers - angle_offset) / CP.steerRatio self.cur_state[0].delta = delta_desired self.angle_steers_des_mpc = float( math.degrees(delta_desired * CP.steerRatio) + angle_offset) # Check for infeasable MPC solution mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta))) t = sec_since_boot() if mpc_nans: self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost) self.cur_state[0].delta = math.radians( angle_steers) / CP.steerRatio if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning("Lateral mpc - nan: True") if self.mpc_solution[ 0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge self.invalid_counter += 1 else: self.invalid_counter = 0 plan_valid = self.invalid_counter < 2 plan_send = messaging.new_message() plan_send.init('pathPlan') plan_send.pathPlan.laneWidth = float(self.MP.lane_width) plan_send.pathPlan.dPoly = map(float, self.MP.d_poly) plan_send.pathPlan.cPoly = map(float, self.MP.c_poly) plan_send.pathPlan.cProb = float(self.MP.c_prob) plan_send.pathPlan.lPoly = map(float, l_poly) plan_send.pathPlan.lProb = float(self.MP.l_prob) plan_send.pathPlan.rPoly = map(float, r_poly) plan_send.pathPlan.rProb = float(self.MP.r_prob) plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc) plan_send.pathPlan.valid = bool(plan_valid) self.plan.send(plan_send.to_bytes()) dat = messaging.new_message() dat.init('liveMpc') dat.liveMpc.x = list(self.mpc_solution[0].x) dat.liveMpc.y = list(self.mpc_solution[0].y) dat.liveMpc.psi = list(self.mpc_solution[0].psi) dat.liveMpc.delta = list(self.mpc_solution[0].delta) dat.liveMpc.cost = self.mpc_solution[0].cost self.livempc.send(dat.to_bytes())
def thermald_thread(): pm = messaging.PubMaster(['deviceState']) pandaState_timeout = int(1000 * 2.5 * DT_TRML) # 2.5x the expected pandaState frequency pandaState_sock = messaging.sub_sock('pandaStates', timeout=pandaState_timeout) sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "managerState"]) fan_speed = 0 count = 0 startup_conditions = { "ignition": False, } startup_conditions_prev = startup_conditions.copy() off_ts = None started_ts = None started_seen = False thermal_status = ThermalStatus.green usb_power = True network_type = NetworkType.none network_strength = NetworkStrength.unknown network_info = None modem_version = None registered_count = 0 nvme_temps = None modem_temps = None current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML) temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML) pandaState_prev = None should_start_prev = False in_car = False handle_fan = None is_uno = False ui_running_prev = False params = Params() power_monitor = PowerMonitoring() no_panda_cnt = 0 HARDWARE.initialize_hardware() thermal_config = HARDWARE.get_thermal_config() # TODO: use PI controller for UNO controller = PIController(k_p=0, k_i=2e-3, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML)) # Leave flag for loggerd to indicate device was left onroad if params.get_bool("IsOnroad"): params.put_bool("BootedOnroad", True) while True: pandaStates = messaging.recv_sock(pandaState_sock, wait=True) sm.update(0) peripheralState = sm['peripheralState'] msg = read_thermal(thermal_config) if pandaStates is not None and len(pandaStates.pandaStates) > 0: pandaState = pandaStates.pandaStates[0] # If we lose connection to the panda, wait 5 seconds before going offroad if pandaState.pandaType == log.PandaState.PandaType.unknown: no_panda_cnt += 1 if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML: if startup_conditions["ignition"]: cloudlog.error("Lost panda connection while onroad") startup_conditions["ignition"] = False else: no_panda_cnt = 0 startup_conditions["ignition"] = pandaState.ignitionLine or pandaState.ignitionCan in_car = pandaState.harnessStatus != log.PandaState.HarnessStatus.notConnected usb_power = peripheralState.usbPowerMode != log.PeripheralState.UsbPowerMode.client # Setup fan handler on first connect to panda if handle_fan is None and peripheralState.pandaType != log.PandaState.PandaType.unknown: is_uno = peripheralState.pandaType == log.PandaState.PandaType.uno if TICI: cloudlog.info("Setting up TICI fan handler") handle_fan = handle_fan_tici elif is_uno or PC: cloudlog.info("Setting up UNO fan handler") handle_fan = handle_fan_uno else: cloudlog.info("Setting up EON fan handler") setup_eon_fan() handle_fan = handle_fan_eon # Handle disconnect if pandaState_prev is not None: if pandaState.pandaType == log.PandaState.PandaType.unknown and \ pandaState_prev.pandaType != log.PandaState.PandaType.unknown: params.clear_all(ParamKeyType.CLEAR_ON_PANDA_DISCONNECT) pandaState_prev = pandaState # these are expensive calls. update every 10s if (count % int(10. / DT_TRML)) == 0: try: network_type = HARDWARE.get_network_type() network_strength = HARDWARE.get_network_strength(network_type) network_info = HARDWARE.get_network_info() # pylint: disable=assignment-from-none nvme_temps = HARDWARE.get_nvme_temperatures() modem_temps = HARDWARE.get_modem_temperatures() # Log modem version once if modem_version is None: modem_version = HARDWARE.get_modem_version() # pylint: disable=assignment-from-none if modem_version is not None: cloudlog.warning(f"Modem version: {modem_version}") if TICI and (network_info.get('state', None) == "REGISTERED"): registered_count += 1 else: registered_count = 0 if registered_count > 10: cloudlog.warning(f"Modem stuck in registered state {network_info}. nmcli conn up lte") os.system("nmcli conn up lte") registered_count = 0 except Exception: cloudlog.exception("Error getting network status") msg.deviceState.freeSpacePercent = get_available_percent(default=100.0) msg.deviceState.memoryUsagePercent = int(round(psutil.virtual_memory().percent)) msg.deviceState.cpuUsagePercent = [int(round(n)) for n in psutil.cpu_percent(percpu=True)] msg.deviceState.gpuUsagePercent = int(round(HARDWARE.get_gpu_usage_percent())) msg.deviceState.networkType = network_type msg.deviceState.networkStrength = network_strength if network_info is not None: msg.deviceState.networkInfo = network_info if nvme_temps is not None: msg.deviceState.nvmeTempC = nvme_temps if modem_temps is not None: msg.deviceState.modemTempC = modem_temps msg.deviceState.batteryPercent = HARDWARE.get_battery_capacity() msg.deviceState.batteryCurrent = HARDWARE.get_battery_current() msg.deviceState.usbOnline = HARDWARE.get_usb_present() current_filter.update(msg.deviceState.batteryCurrent / 1e6) max_comp_temp = temp_filter.update( max(max(msg.deviceState.cpuTempC), msg.deviceState.memoryTempC, max(msg.deviceState.gpuTempC)) ) if handle_fan is not None: fan_speed = handle_fan(controller, max_comp_temp, fan_speed, startup_conditions["ignition"]) msg.deviceState.fanSpeedPercentDesired = fan_speed is_offroad_for_5_min = (started_ts is None) and ((not started_seen) or (off_ts is None) or (sec_since_boot() - off_ts > 60 * 5)) if is_offroad_for_5_min and max_comp_temp > OFFROAD_DANGER_TEMP: # If device is offroad we want to cool down before going onroad # since going onroad increases load and can make temps go over 107 thermal_status = ThermalStatus.danger else: current_band = THERMAL_BANDS[thermal_status] band_idx = list(THERMAL_BANDS.keys()).index(thermal_status) if current_band.min_temp is not None and max_comp_temp < current_band.min_temp: thermal_status = list(THERMAL_BANDS.keys())[band_idx - 1] elif current_band.max_temp is not None and max_comp_temp > current_band.max_temp: thermal_status = list(THERMAL_BANDS.keys())[band_idx + 1] # **** starting logic **** # Check for last update time and display alerts if needed now = datetime.datetime.utcnow() # show invalid date/time alert startup_conditions["time_valid"] = (now.year > 2020) or (now.year == 2020 and now.month >= 10) set_offroad_alert_if_changed("Offroad_InvalidTime", (not startup_conditions["time_valid"])) # Show update prompt try: last_update = datetime.datetime.fromisoformat(params.get("LastUpdateTime", encoding='utf8')) except (TypeError, ValueError): last_update = now dt = now - last_update update_failed_count = params.get("UpdateFailedCount") update_failed_count = 0 if update_failed_count is None else int(update_failed_count) last_update_exception = params.get("LastUpdateException", encoding='utf8') if update_failed_count > 15 and last_update_exception is not None: if tested_branch: extra_text = "Ensure the software is correctly installed" else: extra_text = last_update_exception set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) set_offroad_alert_if_changed("Offroad_UpdateFailed", True, extra_text=extra_text) elif dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1: set_offroad_alert_if_changed("Offroad_UpdateFailed", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", True) elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT: remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0)) set_offroad_alert_if_changed("Offroad_UpdateFailed", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining_time} days.") else: set_offroad_alert_if_changed("Offroad_UpdateFailed", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) startup_conditions["up_to_date"] = params.get("Offroad_ConnectivityNeeded") is None or params.get_bool("DisableUpdates") startup_conditions["not_uninstalling"] = not params.get_bool("DoUninstall") startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version panda_signature = params.get("PandaFirmware") startup_conditions["fw_version_match"] = (panda_signature is None) or (panda_signature == FW_SIGNATURE) # don't show alert is no panda is connected (None) set_offroad_alert_if_changed("Offroad_PandaFirmwareMismatch", (not startup_conditions["fw_version_match"])) # with 2% left, we killall, otherwise the phone will take a long time to boot startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2 startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version or \ params.get_bool("Passive") startup_conditions["not_driver_view"] = not params.get_bool("IsDriverViewEnabled") startup_conditions["not_taking_snapshot"] = not params.get_bool("IsTakingSnapshot") # if any CPU gets above 107 or the battery gets above 63, kill all processes # controls will warn with CPU above 95 or battery above 60 startup_conditions["device_temp_good"] = thermal_status < ThermalStatus.danger set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", (not startup_conditions["device_temp_good"])) if TICI: set_offroad_alert_if_changed("Offroad_NvmeMissing", (not Path("/data/media").is_mount())) # Handle offroad/onroad transition should_start = all(startup_conditions.values()) if should_start != should_start_prev or (count == 0): params.put_bool("IsOnroad", should_start) params.put_bool("IsOffroad", not should_start) HARDWARE.set_power_save(not should_start) if should_start: off_ts = None if started_ts is None: started_ts = sec_since_boot() started_seen = True else: if startup_conditions["ignition"] and (startup_conditions != startup_conditions_prev): cloudlog.event("Startup blocked", startup_conditions=startup_conditions) started_ts = None if off_ts is None: off_ts = sec_since_boot() # Offroad power monitoring power_monitor.calculate(peripheralState, startup_conditions["ignition"]) msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used() msg.deviceState.carBatteryCapacityUwh = max(0, power_monitor.get_car_battery_capacity()) # Check if we need to disable charging (handled by boardd) msg.deviceState.chargingDisabled = power_monitor.should_disable_charging(startup_conditions["ignition"], in_car, off_ts) # Check if we need to shut down if power_monitor.should_shutdown(peripheralState, startup_conditions["ignition"], in_car, off_ts, started_seen): cloudlog.info(f"shutting device down, offroad since {off_ts}") # TODO: add function for blocking cloudlog instead of sleep time.sleep(10) HARDWARE.shutdown() # If UI has crashed, set the brightness to reasonable non-zero value ui_running = "ui" in (p.name for p in sm["managerState"].processes if p.running) if ui_running_prev and not ui_running: HARDWARE.set_screen_brightness(20) ui_running_prev = ui_running msg.deviceState.chargingError = current_filter.x > 0. and msg.deviceState.batteryPercent < 90 # if current is positive, then battery is being discharged msg.deviceState.started = started_ts is not None msg.deviceState.startedMonoTime = int(1e9*(started_ts or 0)) last_ping = params.get("LastAthenaPingTime") if last_ping is not None: msg.deviceState.lastAthenaPingTime = int(last_ping) msg.deviceState.thermalStatus = thermal_status pm.send("deviceState", msg) if EON and not is_uno: set_offroad_alert_if_changed("Offroad_ChargeDisabled", (not usb_power)) should_start_prev = should_start startup_conditions_prev = startup_conditions.copy() # report to server once every 10 minutes if (count % int(600. / DT_TRML)) == 0: if EON and started_ts is None and msg.deviceState.memoryUsagePercent > 40: cloudlog.event("High offroad memory usage", mem=msg.deviceState.memoryUsagePercent) cloudlog.event("STATUS_PACKET", count=count, pandaStates=(strip_deprecated_keys(pandaStates.to_dict()) if pandaStates else None), peripheralState=strip_deprecated_keys(peripheralState.to_dict()), location=(strip_deprecated_keys(sm["gpsLocationExternal"].to_dict()) if sm.alive["gpsLocationExternal"] else None), deviceState=strip_deprecated_keys(msg.to_dict())) count += 1
def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offset, VM, PL): cur_time = sec_since_boot() self.mpc_updated = False # TODO: this creates issues in replay when rewinding time: mpc won't run if self.last_mpc_ts < PL.last_md_ts: self.last_mpc_ts = PL.last_md_ts self.angle_steers_des_prev = self.angle_steers_des_mpc curvature_factor = VM.curvature_factor(v_ego) l_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.l_poly)) r_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.r_poly)) p_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.p_poly)) # account for actuation delay self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, VM.CP.steerRatio, VM.CP.steerActuatorDelay) v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed self.libmpc.run_mpc(self.cur_state, self.mpc_solution, l_poly, r_poly, p_poly, PL.PP.l_prob, PL.PP.r_prob, PL.PP.p_prob, curvature_factor, v_ego_mpc, PL.PP.lane_width) # reset to current steer angle if not active or overriding if active: delta_desired = self.mpc_solution[0].delta[1] else: delta_desired = math.radians(angle_steers - angle_offset) / VM.CP.steerRatio self.cur_state[0].delta = delta_desired self.angle_steers_des_mpc = float( math.degrees(delta_desired * VM.CP.steerRatio) + angle_offset) self.angle_steers_des_time = cur_time self.mpc_updated = True # Check for infeasable MPC solution self.mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta))) t = sec_since_boot() if self.mpc_nans: self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, VM.CP.steerRateCost) self.cur_state[0].delta = math.radians( angle_steers) / VM.CP.steerRatio if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning("Lateral mpc - nan: True") if v_ego < 0.3 or not active: output_steer = 0.0 self.pid.reset() else: # TODO: ideally we should interp, but for tuning reasons we keep the mpc solution # constant for 0.05s. #dt = min(cur_time - self.angle_steers_des_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps #self.angle_steers_des = self.angle_steers_des_prev + (dt / _DT_MPC) * (self.angle_steers_des_mpc - self.angle_steers_des_prev) self.angle_steers_des = self.angle_steers_des_mpc steers_max = get_steer_max(VM.CP, v_ego) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max steer_feedforward = self.angle_steers_des # feedforward desired angle if VM.CP.steerControlType == car.CarParams.SteerControlType.torque: steer_feedforward *= v_ego**2 # proportional to realigning tire momentum (~ lateral accel) deadzone = 0.0 output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone) self.sat_flag = self.pid.saturated return output_steer, float(self.angle_steers_des)
def update(self, sm, pm, CP, VM): if not travis: self.arne_sm.update(0) gas_button_status = self.arne_sm['arne182Status'].gasbuttonstatus if gas_button_status == 1: self.blindspotwait = 10 elif gas_button_status == 2: self.blindspotwait = 30 else: self.blindspotwait = 20 if self.arne_sm['arne182Status'].rightBlindspot: self.blindspotTrueCounterright = 0 else: self.blindspotTrueCounterright = self.blindspotTrueCounterright + 1 if self.arne_sm['arne182Status'].leftBlindspot: self.blindspotTrueCounterleft = 0 else: self.blindspotTrueCounterleft = self.blindspotTrueCounterleft + 1 else: self.blindspotwait = 10 self.blindspotTrueCounterleft = 0 self.blindspotTrueCounterright = 0 v_ego = sm['carState'].vEgo angle_steers = sm['carState'].steeringAngle active = sm['controlsState'].active angle_offset = sm['liveParameters'].angleOffset # Run MPC self.angle_steers_des_prev = self.angle_steers_des_mpc #VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio) #VM.update_params(sm['liveParameters'].stiffnessFactor, CP.steerRatio) VM.update_params(sm['liveParameters'].stiffnessFactor, self.steerRatio) curvature_factor = VM.curvature_factor(v_ego) # Get steerRatio and steerRateCost from kegman.json every x seconds self.mpc_frame += 1 if self.mpc_frame % 500 == 0: # live tuning through /data/openpilot/tune.py overrides interface.py settings self.sR_Boost_new = interp(v_ego, self.sR_BoostBP, self.sR_Boost) self.steerRatio = self.op_params.get('steer_ratio', default=12.0) self.sR = [self.steerRatio, self.steerRatio + self.sR_Boost_new] self.sR_time = 100 self.mpc_frame = 0 if v_ego > 5.0: # boost steerRatio by boost amount if desired steer angle is high self.steerRatio_new = interp(abs(angle_steers), self.sRBP, self.sR) self.sR_delay_counter += 1 if self.sR_delay_counter % self.sR_time != 0: if self.steerRatio_new > self.steerRatio: self.steerRatio = self.steerRatio_new else: self.steerRatio = self.steerRatio_new self.sR_delay_counter = 0 else: self.steerRatio = self.sR[0] print("steerRatio = ", self.steerRatio) self.LP.parse_model(sm['model']) # Lane change logic one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker below_lane_change_speed = v_ego < self.alca_min_speed * CV.MPH_TO_MS if sm['carState'].leftBlinker: self.lane_change_direction = LaneChangeDirection.left elif sm['carState'].rightBlinker: self.lane_change_direction = LaneChangeDirection.right if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or ( not one_blinker ) or (not self.lane_change_enabled) or ( sm['carState'].steeringPressed and ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.right) or (sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.left))): self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none else: if sm['carState'].leftBlinker: lane_change_direction = LaneChangeDirection.left elif sm['carState'].rightBlinker: lane_change_direction = LaneChangeDirection.right #if self.alca_nudge_required: torque_applied = (sm['carState'].steeringPressed and \ ((sm['carState'].steeringTorque > 0 and lane_change_direction == LaneChangeDirection.left and not sm['carState'].leftBlindspot) or \ (sm['carState'].steeringTorque < 0 and lane_change_direction == LaneChangeDirection.right and not sm['carState'].rightBlindspot))) or \ (not self.alca_nudge_required and self.blindspotTrueCounterleft > self.blindspotwait and lane_change_direction == LaneChangeDirection.left) or \ (not self.alca_nudge_required and self.blindspotTrueCounterright > self.blindspotwait and lane_change_direction == LaneChangeDirection.right) #else: # torque_applied = True lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob # State transitions # off if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: self.blindspotTrueCounterleft = 0 self.blindspotTrueCounterright = 0 self.lane_change_state = LaneChangeState.preLaneChange self.lane_change_ll_prob = 1.0 # pre elif self.lane_change_state == LaneChangeState.preLaneChange: if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off self.blindspotTrueCounterleft = 0 self.blindspotTrueCounterright = 0 elif torque_applied: self.lane_change_state = LaneChangeState.laneChangeStarting # starting elif self.lane_change_state == LaneChangeState.laneChangeStarting: # fade out over .2s self.lane_change_ll_prob = max( self.lane_change_ll_prob - 2 * DT_MDL, 0.0) # 98% certainty if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01: self.lane_change_state = LaneChangeState.laneChangeFinishing if (self.arne_sm['arne182Status'].rightBlindspot and lane_change_direction == LaneChangeDirection.right ) or (self.arne_sm['arne182Status'].leftBlindspot and lane_change_direction == LaneChangeDirection.left): self.lane_change_state = LaneChangeState.preLaneChange self.blindspotTrueCounterleft = 0 self.blindspotTrueCounterright = 0 # finishing elif self.lane_change_state == LaneChangeState.laneChangeFinishing: # fade in laneline over 1s self.lane_change_ll_prob = min( self.lane_change_ll_prob + 0.5 * DT_MDL, 1.0) if one_blinker and self.lane_change_ll_prob > 0.99: self.lane_change_state = LaneChangeState.preLaneChange self.blindspotTrueCounterleft = 0 self.blindspotTrueCounterright = 0 elif self.lane_change_ll_prob > 0.99: self.lane_change_state = LaneChangeState.off self.blindspotTrueCounterright = 0 self.blindspotTrueCounterleft = 0 if self.lane_change_state in [ LaneChangeState.off, LaneChangeState.preLaneChange ]: self.lane_change_timer = 0.0 else: self.lane_change_timer += DT_MDL self.prev_one_blinker = one_blinker desire = DESIRES[self.lane_change_direction][self.lane_change_state] # Turn off lanes during lane change if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft: self.LP.l_prob *= self.lane_change_ll_prob self.LP.r_prob *= self.lane_change_ll_prob self.LP.update_d_poly(v_ego) # account for actuation delay self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, self.steerRatio, CP.steerActuatorDelay) #if abs(angle_steers - angle_offset) > 4 and CP.lateralTuning.which() == 'pid': #check if this causes laggy steering # self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed self.libmpc.run_mpc(self.cur_state, self.mpc_solution, list(self.LP.l_poly), list(self.LP.r_poly), list(self.LP.d_poly), self.LP.l_prob, self.LP.r_prob, curvature_factor, v_ego_mpc, self.LP.lane_width) # reset to current steer angle if not active or overriding if active: delta_desired = self.mpc_solution[0].delta[1] rate_desired = math.degrees(self.mpc_solution[0].rate[0] * self.steerRatio) else: delta_desired = math.radians(angle_steers - angle_offset) / self.steerRatio rate_desired = 0.0 self.cur_state[0].delta = delta_desired self.angle_steers_des_mpc = float( math.degrees(delta_desired * self.steerRatio) + angle_offset) # Check for infeasable MPC solution mpc_nans = any(math.isnan(x) for x in self.mpc_solution[0].delta) t = sec_since_boot() if mpc_nans: self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost) self.cur_state[0].delta = math.radians( angle_steers - angle_offset) / self.steerRatio if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning("Lateral mpc - nan: True") if self.mpc_solution[ 0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge self.solution_invalid_cnt += 1 else: self.solution_invalid_cnt = 0 plan_solution_valid = self.solution_invalid_cnt < 2 plan_send = messaging.new_message('pathPlan') plan_send.valid = sm.all_alive_and_valid(service_list=[ 'carState', 'controlsState', 'liveParameters', 'model' ]) plan_send.pathPlan.laneWidth = float(self.LP.lane_width) plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly] plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly] plan_send.pathPlan.lProb = float(self.LP.l_prob) plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly] plan_send.pathPlan.rProb = float(self.LP.r_prob) plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc) plan_send.pathPlan.rateSteers = float(rate_desired) plan_send.pathPlan.angleOffset = float( sm['liveParameters'].angleOffsetAverage) plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid) plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid) plan_send.pathPlan.sensorValid = bool(sm['liveParameters'].sensorValid) plan_send.pathPlan.posenetValid = bool( sm['liveParameters'].posenetValid) or self.posenetValid self.posenetValid = bool(sm['liveParameters'].posenetValid) plan_send.pathPlan.desire = desire plan_send.pathPlan.laneChangeState = self.lane_change_state plan_send.pathPlan.laneChangeDirection = self.lane_change_direction pm.send('pathPlan', plan_send) dat = messaging.new_message('liveMpc') dat.liveMpc.x = list(self.mpc_solution[0].x) dat.liveMpc.y = list(self.mpc_solution[0].y) dat.liveMpc.psi = list(self.mpc_solution[0].psi) dat.liveMpc.delta = list(self.mpc_solution[0].delta) dat.liveMpc.cost = self.mpc_solution[0].cost pm.send('liveMpc', dat) msg = messaging_arne.new_message('latControl') msg.latControl.anglelater = math.degrees( list(self.mpc_solution[0].delta)[-1]) if not travis: self.arne_pm.send('latControl', msg)
def manager_thread(): cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) # save boot log subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "selfdrive/loggerd")) params = Params() ignore = [] if params.get("DongleId", encoding='utf8') == UNREGISTERED_DONGLE_ID: ignore += ["manage_athenad", "uploader"] if os.getenv("NOBOARD") is not None: ignore.append("pandad") if os.getenv("BLOCK") is not None: ignore += os.getenv("BLOCK").split(",") ensure_running(managed_processes.values(), started=False, not_run=ignore) started_prev = False sm = messaging.SubMaster(['deviceState']) pm = messaging.PubMaster(['managerState']) while True: sm.update() not_run = ignore[:] if sm['deviceState'].freeSpacePercent < 5: not_run.append("loggerd") started = sm['deviceState'].started driverview = params.get_bool("IsDriverViewEnabled") ensure_running(managed_processes.values(), started, driverview, not_run) # trigger an update after going offroad if started_prev and not started and 'updated' in managed_processes: os.sync() managed_processes['updated'].signal(signal.SIGHUP) started_prev = started running = ' '.join([ "%s%s\u001b[0m" % ("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name) for p in managed_processes.values() if p.proc ]) print(running) cloudlog.debug(running) # send managerState msg = messaging.new_message('managerState') msg.managerState.processes = [ p.get_process_state_msg() for p in managed_processes.values() ] pm.send('managerState', msg) # Exit main loop when uninstall/shutdown/reboot is needed shutdown = False for param in ("DoUninstall", "DoShutdown", "DoReboot"): if params.get_bool(param): cloudlog.warning(f"Shutting down manager - {param} set") shutdown = True if shutdown: break
def register(show_spinner=False) -> str: params = Params() params.put("SubscriberInfo", HARDWARE.get_subscriber_info()) IMEI = params.get("IMEI", encoding='utf8') HardwareSerial = params.get("HardwareSerial", encoding='utf8') dongle_id = params.get("DongleId", encoding='utf8') needs_registration = None in (IMEI, HardwareSerial, dongle_id) pubkey = Path(PERSIST + "/comma/id_rsa.pub") if not pubkey.is_file(): dongle_id = UNREGISTERED_DONGLE_ID cloudlog.warning(f"missing public key: {pubkey}") elif needs_registration: if show_spinner: spinner = Spinner() spinner.update("registering device") # Create registration token, in the future, this key will make JWTs directly with open(PERSIST + "/comma/id_rsa.pub") as f1, open(PERSIST + "/comma/id_rsa") as f2: public_key = f1.read() private_key = f2.read() # Block until we get the imei serial = HARDWARE.get_serial() start_time = time.monotonic() imei1, imei2 = None, None while imei1 is None and imei2 is None: try: imei1, imei2 = HARDWARE.get_imei(0), HARDWARE.get_imei(1) except Exception: cloudlog.exception("Error getting imei, trying again...") time.sleep(1) if time.monotonic() - start_time > 60 and show_spinner: spinner.update( f"registering device - serial: {serial}, IMEI: ({imei1}, {imei2})" ) params.put("IMEI", imei1) params.put("HardwareSerial", serial) backoff = 0 start_time = time.monotonic() while True: try: register_token = jwt.encode( { 'register': True, 'exp': datetime.utcnow() + timedelta(hours=1) }, private_key, algorithm='RS256') cloudlog.info("getting pilotauth") resp = api_get("v2/pilotauth/", method='POST', timeout=15, imei=imei1, imei2=imei2, serial=serial, public_key=public_key, register_token=register_token) if resp.status_code in (402, 403): cloudlog.info( f"Unable to register device, got {resp.status_code}") dongle_id = UNREGISTERED_DONGLE_ID else: dongleauth = json.loads(resp.text) dongle_id = dongleauth["dongle_id"] break except Exception: cloudlog.exception("failed to authenticate") backoff = min(backoff + 1, 15) time.sleep(backoff) if time.monotonic() - start_time > 60 and show_spinner: spinner.update( f"registering device - serial: {serial}, IMEI: ({imei1}, {imei2})" ) if show_spinner: spinner.close() if dongle_id: params.put("DongleId", dongle_id) set_offroad_alert("Offroad_UnofficialHardware", dongle_id == UNREGISTERED_DONGLE_ID) return dongle_id
def uninstall(): cloudlog.warning("uninstalling") with open('/cache/recovery/command', 'w') as f: f.write('--wipe_data\n') # IPowerManager.reboot(confirm=false, reason="recovery", wait=true) HARDWARE.reboot(reason="recovery")
def manager_thread(): # now loop context = zmq.Context() thermal_sock = messaging.pub_sock(context, service_list['thermal'].port) health_sock = messaging.sub_sock(context, service_list['health'].port) location_sock = messaging.sub_sock(context, service_list['gpsLocation'].port) cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) for p in persistent_processes: start_managed_process(p) # start frame system("am start -n ai.comma.plus.frame/.MainActivity") # do this before panda flashing setup_eon_fan() if os.getenv("NOBOARD") is None: start_managed_process("pandad") params = Params() passive_starter = LocationStarter() started_ts = None off_ts = None logger_dead = False count = 0 fan_speed = 0 ignition_seen = False started_seen = False panda_seen = False health_sock.RCVTIMEO = 1500 while 1: # get health of board, log this in "thermal" td = messaging.recv_sock(health_sock, wait=True) location = messaging.recv_sock(location_sock) location = location.gpsLocation if location else None print td # replace thermald msg = read_thermal() # loggerd is gated based on free space statvfs = os.statvfs(ROOT) avail = (statvfs.f_bavail * 1.0) / statvfs.f_blocks # thermal message now also includes free space msg.thermal.freeSpace = avail with open("/sys/class/power_supply/battery/capacity") as f: msg.thermal.batteryPercent = int(f.read()) with open("/sys/class/power_supply/battery/status") as f: msg.thermal.batteryStatus = f.read().strip() with open("/sys/class/power_supply/usb/online") as f: msg.thermal.usbOnline = bool(int(f.read())) # TODO: add car battery voltage check max_temp = max(msg.thermal.cpu0, msg.thermal.cpu1, msg.thermal.cpu2, msg.thermal.cpu3) / 10.0 bat_temp = msg.thermal.bat / 1000. fan_speed = handle_fan(max_temp, bat_temp, fan_speed) msg.thermal.fanSpeed = fan_speed msg.thermal.started = started_ts is not None msg.thermal.startedTs = int(1e9 * (started_ts or 0)) thermal_sock.send(msg.to_bytes()) print msg # uploader is gated based on the phone temperature if max_temp > 85.0: cloudlog.warning("over temp: %r", max_temp) kill_managed_process("uploader") elif max_temp < 70.0: start_managed_process("uploader") if avail < 0.05: logger_dead = True # start constellation of processes when the car starts ignition = td is not None and td.health.started ignition_seen = ignition_seen or ignition # add voltage check for ignition if not ignition_seen and td is not None and td.health.voltage > 13500: ignition = True do_uninstall = params.get("DoUninstall") == "1" accepted_terms = params.get("HasAcceptedTerms") == "1" completed_training = params.get( "CompletedTrainingVersion") == training_version should_start = ignition # have we seen a panda? panda_seen = panda_seen or td is not None # start on gps movement if we haven't seen ignition and are in passive mode should_start = should_start or ( not (ignition_seen and td) # seen ignition and panda is connected and params.get("Passive") == "1" and passive_starter.update(started_ts, location)) # with 2% left, we killall, otherwise the phone will take a long time to boot should_start = should_start and avail > 0.02 # require usb power should_start = should_start and msg.thermal.usbOnline should_start = should_start and accepted_terms and completed_training and ( not do_uninstall) # if any CPU gets above 107 or the battery gets above 63, kill all processes # controls will warn with CPU above 95 or battery above 60 if max_temp > 107.0 or msg.thermal.bat >= 63000: # TODO: Add a better warning when this is happening should_start = False if should_start: off_ts = None if started_ts is None: params.car_start() started_ts = sec_since_boot() started_seen = True for p in car_started_processes: if p == "loggerd" and logger_dead: kill_managed_process(p) else: start_managed_process(p) else: started_ts = None if off_ts is None: off_ts = sec_since_boot() logger_dead = False for p in car_started_processes: kill_managed_process(p) # shutdown if the battery gets lower than 3%, t's discharging, we aren't running for # more than a minute but we were running if msg.thermal.batteryPercent < 3 and msg.thermal.batteryStatus == "Discharging" and \ started_seen and (sec_since_boot() - off_ts) > 60: os.system('LD_LIBRARY_PATH="" svc power shutdown') # check the status of all processes, did any of them die? for p in running: cloudlog.debug(" running %s %s" % (p, running[p])) # report to server once per minute if (count % 60) == 0: cloudlog.event("STATUS_PACKET", running=running.keys(), count=count, health=(td.to_dict() if td else None), location=(location.to_dict() if location else None), thermal=msg.to_dict()) if do_uninstall: break count += 1
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) ret.carName = "toyota" ret.safetyModel = car.CarParams.SafetyModel.toyota ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay ret.steerLimitTimer = 0.4 if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H]: # These cars use LQR/INDI ret.lateralTuning.init('pid') ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] if candidate == CAR.PRIUS: stop_and_go = True ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.70 ret.steerRatio = 15.74 # unknown end-to-end spec tire_stiffness_factor = 0.6371 # hand-tune ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGain = 4.0 ret.lateralTuning.indi.outerLoopGain = 3.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.0 ret.steerActuatorDelay = 0.5 elif candidate in [CAR.RAV4, CAR.RAV4H]: stop_and_go = True if (candidate in CAR.RAV4H) else False ret.safetyParam = 73 ret.wheelbase = 2.65 ret.steerRatio = 16.88 # 14.5 is spec end-to-end tire_stiffness_factor = 0.5533 ret.mass = 3650. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = 1500.0 ret.lateralTuning.lqr.ki = 0.05 ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] ret.lateralTuning.lqr.c = [1., 0.] ret.lateralTuning.lqr.k = [-110.73572306, 451.22718255] ret.lateralTuning.lqr.l = [0.3233671, 0.3185757] ret.lateralTuning.lqr.dcGain = 0.002237852961363602 elif candidate == CAR.COROLLA: stop_and_go = False ret.safetyParam = 100 ret.wheelbase = 2.70 ret.steerRatio = 18.27 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]] ret.lateralTuning.pid.kf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594 elif candidate == CAR.LEXUS_RX: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.79 ret.steerRatio = 14.8 tire_stiffness_factor = 0.5533 ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.05]] ret.lateralTuning.pid.kf = 0.00006 elif candidate == CAR.LEXUS_RXH: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.79 ret.steerRatio = 16. # 14.8 is spec end-to-end tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 4481. * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.lateralTuning.pid.kf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 elif candidate == CAR.LEXUS_RX_TSS2: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.79 ret.steerRatio = 14.8 tire_stiffness_factor = 0.5533 # not optimized yet ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.lateralTuning.pid.kf = 0.00007818594 elif candidate == CAR.LEXUS_RXH_TSS2: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.79 ret.steerRatio = 16.0 # 14.8 is spec end-to-end tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 4481.0 * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.15]] ret.lateralTuning.pid.kf = 0.00007818594 elif candidate in [CAR.CHR, CAR.CHRH]: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.63906 ret.steerRatio = 13.6 tire_stiffness_factor = 0.7933 ret.mass = 3300. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.723], [0.0428]] ret.lateralTuning.pid.kf = 0.00006 elif candidate in [CAR.CAMRY, CAR.CAMRYH]: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.82448 ret.steerRatio = 13.7 tire_stiffness_factor = 0.7933 ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.lateralTuning.pid.kf = 0.00006 elif candidate in [CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2]: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.84988 # 112.2 in = 2.84988 m ret.steerRatio = 16.0 tire_stiffness_factor = 0.8 ret.mass = 4700. * CV.LB_TO_KG + STD_CARGO_KG # 4260 + 4-5 people ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[ 0.18 ], [0.015]] # community tuning ret.lateralTuning.pid.kf = 0.00012 # community tuning elif candidate in [CAR.HIGHLANDER, CAR.HIGHLANDERH]: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.78 ret.steerRatio = 16.0 tire_stiffness_factor = 0.8 ret.mass = 4607. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid limited ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[ 0.18 ], [0.015]] # community tuning ret.lateralTuning.pid.kf = 0.00012 # community tuning elif candidate == CAR.AVALON: stop_and_go = False ret.safetyParam = 73 ret.wheelbase = 2.82 ret.steerRatio = 14.8 # Found at https://pressroom.toyota.com/releases/2016+avalon+product+specs.download tire_stiffness_factor = 0.7983 ret.mass = 3505. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.17], [0.03]] ret.lateralTuning.pid.kf = 0.00006 elif candidate == CAR.RAV4_TSS2: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.68986 ret.steerRatio = 14.3 tire_stiffness_factor = 0.7933 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15], [0.05]] ret.mass = 3370. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kf = 0.00004 for fw in car_fw: if fw.ecu == "eps" and fw.fwVersion == b"8965B42170\x00\x00\x00\x00\x00\x00": ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[ 0.6 ], [0.1]] ret.lateralTuning.pid.kf = 0.00007818594 break elif candidate == CAR.RAV4H_TSS2: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.68986 ret.steerRatio = 14.3 tire_stiffness_factor = 0.7933 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15], [0.05]] ret.mass = 3800. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kf = 0.00004 for fw in car_fw: if fw.ecu == "eps" and fw.fwVersion == b"8965B42170\x00\x00\x00\x00\x00\x00": ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[ 0.6 ], [0.1]] ret.lateralTuning.pid.kf = 0.00007818594 break elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2]: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.63906 ret.steerRatio = 13.9 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.lateralTuning.pid.kf = 0.00007818594 elif candidate in [CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2]: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.8702 ret.steerRatio = 16.0 # not optimized tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 3704. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.lateralTuning.pid.kf = 0.00007818594 elif candidate == CAR.SIENNA: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 3.03 ret.steerRatio = 15.5 tire_stiffness_factor = 0.444 ret.mass = 4590. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.02]] ret.lateralTuning.pid.kf = 0.00007818594 elif candidate == CAR.LEXUS_IS: stop_and_go = False ret.safetyParam = 77 ret.wheelbase = 2.79908 ret.steerRatio = 13.3 tire_stiffness_factor = 0.444 ret.mass = 3736.8 * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]] ret.lateralTuning.pid.kf = 0.00006 elif candidate == CAR.LEXUS_CTH: stop_and_go = True ret.safetyParam = 100 ret.wheelbase = 2.60 ret.steerRatio = 18.6 tire_stiffness_factor = 0.517 ret.mass = 3108 * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]] ret.lateralTuning.pid.kf = 0.00007 elif candidate == CAR.LEXUS_NXH: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.66 ret.steerRatio = 14.7 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 4070 * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.lateralTuning.pid.kf = 0.00006 ret.steerRateCost = 1. ret.centerToFront = ret.wheelbase * 0.44 # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness( ret.mass, ret.wheelbase, ret.centerToFront, tire_stiffness_factor=tire_stiffness_factor) ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay # Detect smartDSU, which intercepts ACC_CMD from the DSU allowing openpilot to send it smartDsu = 0x2FF in fingerprint[0] # In TSS2 cars the camera does long control ret.enableDsu = is_ecu_disconnected( fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.dsu) and candidate not in TSS2_CAR ret.enableGasInterceptor = 0x201 in fingerprint[0] # if the smartDSU is detected, openpilot can send ACC_CMD (and the smartDSU will block it from the DSU) or not (the DSU is "connected") ret.openpilotLongitudinalControl = ret.enableCamera and ( smartDsu or ret.enableDsu or candidate in TSS2_CAR) cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera) cloudlog.warning("ECU DSU Simulated: %r", ret.enableDsu) cloudlog.warning("ECU Gas Interceptor: %r", ret.enableGasInterceptor) # min speed to enable ACC. if car can do stop and go, then set enabling speed # to a negative value, so it won't matter. ret.minEnableSpeed = -1. if ( stop_and_go or ret.enableGasInterceptor) else 19. * CV.MPH_TO_MS # removing the DSU disables AEB and it's considered a community maintained feature # intercepting the DSU is a community feature since it requires unofficial hardware ret.communityFeature = ret.enableGasInterceptor or ret.enableDsu or smartDsu ret.longitudinalTuning.deadzoneBP = [0., 9.] ret.longitudinalTuning.deadzoneV = [0., .15] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kiBP = [0., 35.] if ret.enableGasInterceptor: ret.gasMaxBP = [0., 9., 35] ret.gasMaxV = [0.2, 0.5, 0.7] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] ret.longitudinalTuning.kiV = [0.18, 0.12] else: ret.gasMaxBP = [0.] ret.gasMaxV = [0.5] ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] ret.longitudinalTuning.kiV = [0.54, 0.36] return ret
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): # pylint: disable=dangerous-default-value ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) ret.carName = "honda" if candidate in HONDA_BOSCH: ret.safetyModel = car.CarParams.SafetyModel.hondaBoschHarness if has_relay else car.CarParams.SafetyModel.hondaBoschGiraffe rdr_bus = 0 if has_relay else 2 ret.enableCamera = is_ecu_disconnected( fingerprint[rdr_bus], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay ret.radarOffCan = True ret.openpilotLongitudinalControl = False else: ret.safetyModel = car.CarParams.SafetyModel.hondaNidec ret.enableCamera = is_ecu_disconnected( fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay ret.enableGasInterceptor = 0x201 in fingerprint[0] ret.openpilotLongitudinalControl = ret.enableCamera cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera) cloudlog.warning("ECU Gas Interceptor: %r", ret.enableGasInterceptor) ret.enableCruise = not ret.enableGasInterceptor ret.communityFeature = ret.enableGasInterceptor # Certain Hondas have an extra steering sensor at the bottom of the steering rack, # which improves controls quality as it removes the steering column torsion from feedback. # Tire stiffness factor fictitiously lower if it includes the steering column torsion effect. # For modeling details, see p.198-200 in "The Science of Vehicle Dynamics (2014), M. Guiggiani" ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0], [0]] ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kf = 0.00006 # conservative feed-forward eps_modified = False for fw in car_fw: if fw.ecu == "eps" and b"," in fw.fwVersion: eps_modified = True if candidate == CAR.CIVIC: stop_and_go = True ret.mass = CivicParams.MASS ret.wheelbase = CivicParams.WHEELBASE ret.centerToFront = CivicParams.CENTER_TO_FRONT ret.steerRatio = 15.38 # 10.93 is end-to-end spec if eps_modified: # stock request input values: 0x0000, 0x00DE, 0x014D, 0x01EF, 0x0290, 0x0377, 0x0454, 0x0610, 0x06EE # stock request output values: 0x0000, 0x0917, 0x0DC5, 0x1017, 0x119F, 0x140B, 0x1680, 0x1680, 0x1680 # modified request output values: 0x0000, 0x0917, 0x0DC5, 0x1017, 0x119F, 0x140B, 0x1680, 0x2880, 0x3180 # stock filter output values: 0x009F, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108 # modified filter output values: 0x009F, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0400, 0x0480 # note: max request allowed is 4096, but request is capped at 3840 in firmware, so modifications result in 2x max ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[ 0, 2560, 8000 ], [0, 2560, 3840]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.1]] else: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[ 0, 2560 ], [0, 2560]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.1], [0.33]] tire_stiffness_factor = 1. ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] ret.longitudinalTuning.kiBP = [0., 35.] ret.longitudinalTuning.kiV = [0.54, 0.36] elif candidate in (CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL): stop_and_go = True ret.mass = CivicParams.MASS ret.wheelbase = CivicParams.WHEELBASE ret.centerToFront = CivicParams.CENTER_TO_FRONT ret.steerRatio = 15.38 # 10.93 is end-to-end spec ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [ [0, 4096], [0, 4096] ] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 1. ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] ret.longitudinalTuning.kiBP = [0., 35.] ret.longitudinalTuning.kiV = [0.18, 0.12] elif candidate in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH): stop_and_go = True if not candidate == CAR.ACCORDH: # Hybrid uses same brake msg as hatch ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg ret.mass = 3279. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.83 ret.centerToFront = ret.wheelbase * 0.39 ret.steerRatio = 16.33 # 11.82 is spec end-to-end ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [ [0, 4096], [0, 4096] ] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.8467 ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] ret.longitudinalTuning.kiBP = [0., 35.] ret.longitudinalTuning.kiV = [0.18, 0.12] if eps_modified: ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.09]] else: ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] elif candidate == CAR.ACURA_ILX: stop_and_go = False ret.mass = 3095. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.67 ret.centerToFront = ret.wheelbase * 0.37 ret.steerRatio = 18.61 # 15.3 is spec end-to-end ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [ [0, 3840], [0, 3840] ] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.72 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] ret.longitudinalTuning.kiBP = [0., 35.] ret.longitudinalTuning.kiV = [0.18, 0.12] elif candidate in (CAR.CRV, CAR.CRV_EU): stop_and_go = False ret.mass = 3572. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.62 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.89 # as spec ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [ [0, 1000], [0, 1000] ] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.444 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] ret.longitudinalTuning.kiBP = [0., 35.] ret.longitudinalTuning.kiV = [0.18, 0.12] elif candidate == CAR.CRV_5G: stop_and_go = True ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg ret.mass = 3410. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.66 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.0 # 12.3 is spec end-to-end if eps_modified: # stock request input values: 0x0000, 0x00DB, 0x01BB, 0x0296, 0x0377, 0x0454, 0x0532, 0x0610, 0x067F # stock request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x129A, 0x134D, 0x1400 # modified request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x1ACD, 0x239A, 0x2800 ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[ 0, 2560, 10000 ], [0, 2560, 3840]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.1]] else: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[ 0, 3840 ], [0, 3840]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[ 0.64 ], [0.192]] tire_stiffness_factor = 0.677 ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] ret.longitudinalTuning.kiBP = [0., 35.] ret.longitudinalTuning.kiV = [0.18, 0.12] elif candidate == CAR.CRV_HYBRID: stop_and_go = True ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg ret.mass = 1667. + STD_CARGO_KG # mean of 4 models in kg ret.wheelbase = 2.66 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.0 # 12.3 is spec end-to-end ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [ [0, 4096], [0, 4096] ] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.677 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] ret.longitudinalTuning.kiBP = [0., 35.] ret.longitudinalTuning.kiV = [0.18, 0.12] elif candidate == CAR.FIT: stop_and_go = False ret.mass = 2644. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.53 ret.centerToFront = ret.wheelbase * 0.39 ret.steerRatio = 13.06 ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [ [0, 4096], [0, 4096] ] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.75 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.06]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] ret.longitudinalTuning.kiBP = [0., 35.] ret.longitudinalTuning.kiV = [0.18, 0.12] elif candidate == CAR.HRV: stop_and_go = False ret.mass = 3125 * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.61 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 15.2 ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] tire_stiffness_factor = 0.5 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.025]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] ret.longitudinalTuning.kiBP = [0., 35.] ret.longitudinalTuning.kiV = [0.18, 0.12] elif candidate == CAR.ACURA_RDX: stop_and_go = False ret.mass = 3935. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.68 ret.centerToFront = ret.wheelbase * 0.38 ret.steerRatio = 15.0 # as spec ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [ [0, 1000], [0, 1000] ] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.444 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] ret.longitudinalTuning.kiBP = [0., 35.] ret.longitudinalTuning.kiV = [0.18, 0.12] elif candidate == CAR.ODYSSEY: stop_and_go = False ret.mass = 4471. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 3.00 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 14.35 # as spec ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [ [0, 4096], [0, 4096] ] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.82 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.45], [0.135]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] ret.longitudinalTuning.kiBP = [0., 35.] ret.longitudinalTuning.kiV = [0.18, 0.12] elif candidate == CAR.ODYSSEY_CHN: stop_and_go = False ret.mass = 1849.2 + STD_CARGO_KG # mean of 4 models in kg ret.wheelbase = 2.90 ret.centerToFront = ret.wheelbase * 0.41 # from CAR.ODYSSEY ret.steerRatio = 14.35 ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [ [0, 32767], [0, 32767] ] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.82 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.45], [0.135]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] ret.longitudinalTuning.kiBP = [0., 35.] ret.longitudinalTuning.kiV = [0.18, 0.12] elif candidate in (CAR.PILOT, CAR.PILOT_2019): stop_and_go = False ret.mass = 4204. * CV.LB_TO_KG + STD_CARGO_KG # average weight ret.wheelbase = 2.82 ret.centerToFront = ret.wheelbase * 0.428 ret.steerRatio = 17.25 # as spec ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [ [0, 4096], [0, 4096] ] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.444 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] ret.longitudinalTuning.kiBP = [0., 35.] ret.longitudinalTuning.kiV = [0.18, 0.12] elif candidate == CAR.RIDGELINE: stop_and_go = False ret.mass = 4515. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 3.18 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 15.59 # as spec ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [ [0, 4096], [0, 4096] ] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.444 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] ret.longitudinalTuning.kiBP = [0., 35.] ret.longitudinalTuning.kiV = [0.18, 0.12] elif candidate == CAR.INSIGHT: stop_and_go = True ret.mass = 2987. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.7 ret.centerToFront = ret.wheelbase * 0.39 ret.steerRatio = 15.0 # 12.58 is spec end-to-end ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [ [0, 4096], [0, 4096] ] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.82 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] ret.longitudinalTuning.kiBP = [0., 35.] ret.longitudinalTuning.kiV = [0.18, 0.12] else: raise ValueError("unsupported car %s" % candidate) # min speed to enable ACC. if car can do stop and go, then set enabling speed # to a negative value, so it won't matter. Otherwise, add 0.5 mph margin to not # conflict with PCM acc ret.minEnableSpeed = -1. if ( stop_and_go or ret.enableGasInterceptor) else 25.5 * CV.MPH_TO_MS # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness( ret.mass, ret.wheelbase, ret.centerToFront, tire_stiffness_factor=tire_stiffness_factor) ret.gasMaxBP = [0.] # m/s ret.gasMaxV = [0.6] if ret.enableGasInterceptor else [ 0. ] # max gas allowed ret.brakeMaxBP = [5., 20.] # m/s ret.brakeMaxV = [1., 0.8] # max brake allowed ret.stoppingControl = True ret.startAccel = 0.5 ret.steerActuatorDelay = 0.1 ret.steerRateCost = 0.5 ret.steerLimitTimer = 0.8 return ret
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False): ret = car.CarParams.new_message() ret.carName = "ford" ret.carFingerprint = candidate ret.carVin = vin ret.isPandaBlack = has_relay ret.safetyModel = car.CarParams.SafetyModel.ford ret.dashcamOnly = True # pedal ret.enableCruise = True ret.wheelbase = 2.85 ret.steerRatio = 14.8 ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01], [ 0.005 ]] # TODO: tune this ret.lateralTuning.pid.kf = 1. / MAX_ANGLE # MAX Steer angle to normalize FF ret.steerActuatorDelay = 0.1 # Default delay, not measured yet ret.steerLimitTimer = 0.8 ret.steerRateCost = 1.0 ret.centerToFront = ret.wheelbase * 0.44 tire_stiffness_factor = 0.5328 # min speed to enable ACC. if car can do stop and go, then set enabling speed # to a negative value, so it won't matter. ret.minEnableSpeed = -1. # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness( ret.mass, ret.wheelbase, ret.centerToFront, tire_stiffness_factor=tire_stiffness_factor) # no rear steering, at least on the listed cars above ret.steerRatioRear = 0. ret.steerControlType = car.CarParams.SteerControlType.angle # steer, gas, brake limitations VS speed ret.steerMaxBP = [0.] # breakpoints at 1 and 40 kph ret.steerMaxV = [1.0] # 2/3rd torque allowed above 45 kph ret.gasMaxBP = [0.] ret.gasMaxV = [0.5] ret.brakeMaxBP = [5., 20.] ret.brakeMaxV = [1., 0.8] ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay ret.openpilotLongitudinalControl = False cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera) ret.stoppingControl = False ret.startAccel = 0.0 ret.longitudinalTuning.deadzoneBP = [0., 9.] ret.longitudinalTuning.deadzoneV = [0., .15] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] ret.longitudinalTuning.kiBP = [0., 35.] ret.longitudinalTuning.kiV = [0.54, 0.36] return ret
def update(self, sm, CP): self.use_lanelines = Params().get('EndToEndToggle') != b'1' self.laneless_mode = int(Params().get("LanelessMode", encoding='utf8')) self.v_cruise_kph = sm['controlsState'].vCruise self.stand_still = sm['carState'].standStill try: lateral_control_method = 0 lateral_control_method = int( sm['controlsState'].lateralControlMethod) if lateral_control_method == 0: self.output_scale = sm[ 'controlsState'].lateralControlState.pidState.output elif lateral_control_method == 1: self.output_scale = sm[ 'controlsState'].lateralControlState.indiState.output elif lateral_control_method == 2: self.output_scale = sm[ 'controlsState'].lateralControlState.lqrState.output except: pass v_ego = sm['carState'].vEgo active = sm['controlsState'].active measured_curvature = sm['controlsState'].curvature md = sm['modelV2'] self.LP.parse_model(sm['modelV2'], sm, v_ego) if len(md.position.x) == TRAJECTORY_SIZE and len( md.orientation.x) == TRAJECTORY_SIZE: self.path_xyz = np.column_stack( [md.position.x, md.position.y, md.position.z]) self.t_idxs = np.array(md.position.t) self.plan_yaw = list(md.orientation.z) # Lane change logic one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN if sm['carState'].leftBlinker: self.lane_change_direction = LaneChangeDirection.left elif sm['carState'].rightBlinker: self.lane_change_direction = LaneChangeDirection.right if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or ( abs(self.output_scale) >= 0.9 and self.lane_change_timer > 1): self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none else: torque_applied = sm['carState'].steeringPressed and \ ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or (sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) blindspot_detected = ( (sm['carState'].leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or (sm['carState'].rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob # State transitions # off if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: self.lane_change_state = LaneChangeState.preLaneChange self.lane_change_ll_prob = 1.0 self.lane_change_wait_timer = 0 # pre elif self.lane_change_state == LaneChangeState.preLaneChange: self.lane_change_wait_timer += DT_MDL if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off elif not blindspot_detected and ( torque_applied or (self.lane_change_auto_delay and self.lane_change_wait_timer > self.lane_change_auto_delay)): self.lane_change_state = LaneChangeState.laneChangeStarting # starting elif self.lane_change_state == LaneChangeState.laneChangeStarting: # fade out over .5s self.lane_change_adjust_new = interp( v_ego, self.lane_change_adjust_vel, self.lane_change_adjust) self.lane_change_ll_prob = max( self.lane_change_ll_prob - self.lane_change_adjust_new * DT_MDL, 0.0) # 98% certainty if lane_change_prob < 0.03 and self.lane_change_ll_prob < 0.02: self.lane_change_state = LaneChangeState.laneChangeFinishing # finishing elif self.lane_change_state == LaneChangeState.laneChangeFinishing: # fade in laneline over 1s self.lane_change_ll_prob = min( self.lane_change_ll_prob + DT_MDL, 1.0) if one_blinker and self.lane_change_ll_prob > 0.98: self.lane_change_state = LaneChangeState.preLaneChange elif self.lane_change_ll_prob > 0.98: self.lane_change_state = LaneChangeState.off if self.lane_change_state in [ LaneChangeState.off, LaneChangeState.preLaneChange ]: self.lane_change_timer = 0.0 else: self.lane_change_timer += DT_MDL self.prev_one_blinker = one_blinker self.desire = DESIRES[self.lane_change_direction][ self.lane_change_state] # Turn off lanes during lane change if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft: self.LP.lll_prob *= self.lane_change_ll_prob self.LP.rll_prob *= self.lane_change_ll_prob if self.use_lanelines: d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz) self.laneless_mode_status = False elif not self.use_lanelines and self.laneless_mode == 0: d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz) self.laneless_mode_status = False # use laneless, it might mitigate abrubt steering at stopping? elif not self.use_lanelines and sm[ 'radarState'].leadOne.dRel < 25 and sm[ 'radarState'].leadOne.vRel < 0 and ( abs(sm['controlsState'].steeringAngleDesiredDeg) - abs(sm['carState'].steeringAngleDeg) ) > 2.5 and self.lane_change_state == LaneChangeState.off: d_path_xyz = self.path_xyz self.laneless_mode_status = True self.laneless_mode_at_stopping = True self.laneless_mode_at_stopping_timer = 200 elif self.laneless_mode_at_stopping and ( v_ego < 1 or not self.laneless_mode_at_stopping_timer): d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz) self.laneless_mode_status = False self.laneless_mode_at_stopping = False elif not self.use_lanelines and self.laneless_mode == 1: d_path_xyz = self.path_xyz self.laneless_mode_status = True elif not self.use_lanelines and self.laneless_mode == 2 and ( self.LP.lll_prob < 0.3 or self.LP.rll_prob < 0.3 ) and self.lane_change_state == LaneChangeState.off: d_path_xyz = self.path_xyz self.laneless_mode_status = True self.laneless_mode_status_buffer = True elif not self.use_lanelines and self.laneless_mode == 2 and ( self.LP.lll_prob > 0.5 and self.LP.rll_prob > 0.5 ) and self.laneless_mode_status_buffer and not self.laneless_mode_at_stopping and self.lane_change_state == LaneChangeState.off: d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz) self.laneless_mode_status = False self.laneless_mode_status_buffer = False elif not self.use_lanelines and self.laneless_mode == 2 and self.laneless_mode_status_buffer == True and self.lane_change_state == LaneChangeState.off: d_path_xyz = self.path_xyz self.laneless_mode_status = True else: d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz) self.laneless_mode_status = False self.laneless_mode_status_buffer = False if self.laneless_mode_at_stopping_timer > 0: self.laneless_mode_at_stopping_timer -= 1 y_pts = np.interp(v_ego * self.t_idxs[:MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1]) heading_pts = np.interp(v_ego * self.t_idxs[:MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw) self.y_pts = y_pts assert len(y_pts) == MPC_N + 1 assert len(heading_pts) == MPC_N + 1 self.libmpc.run_mpc(self.cur_state, self.mpc_solution, float(v_ego), CAR_ROTATION_RADIUS, list(y_pts), list(heading_pts)) # init state for next self.cur_state.x = 0.0 self.cur_state.y = 0.0 self.cur_state.psi = 0.0 self.cur_state.curvature = interp(DT_MDL, self.t_idxs[:MPC_N + 1], self.mpc_solution.curvature) # TODO this needs more thought, use .2s extra for now to estimate other delays delay = CP.steerActuatorDelay + .2 current_curvature = self.mpc_solution.curvature[0] psi = interp(delay, self.t_idxs[:MPC_N + 1], self.mpc_solution.psi) next_curvature_rate = self.mpc_solution.curvature_rate[0] # MPC can plan to turn the wheel and turn back before t_delay. This means # in high delay cases some corrections never even get commanded. So just use # psi to calculate a simple linearization of desired curvature curvature_diff_from_psi = psi / (max(v_ego, 1e-1) * delay) - current_curvature next_curvature = current_curvature + 2 * curvature_diff_from_psi self.desired_curvature = next_curvature self.desired_curvature_rate = next_curvature_rate max_curvature_rate = interp(v_ego, MAX_CURVATURE_RATE_SPEEDS, MAX_CURVATURE_RATES) self.safe_desired_curvature_rate = clip(self.desired_curvature_rate, -max_curvature_rate, max_curvature_rate) self.safe_desired_curvature = clip( self.desired_curvature, self.safe_desired_curvature - max_curvature_rate / DT_MDL, self.safe_desired_curvature + max_curvature_rate / DT_MDL) # Check for infeasable MPC solution mpc_nans = any(math.isnan(x) for x in self.mpc_solution.curvature) t = sec_since_boot() if mpc_nans: self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, CP.steerRateCost) self.cur_state.curvature = measured_curvature if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning("Lateral mpc - nan: True") if self.mpc_solution[ 0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge self.solution_invalid_cnt += 1 else: self.solution_invalid_cnt = 0
def update(self, sm, CP, VM): v_ego = sm['carState'].vEgo active = sm['controlsState'].active steering_wheel_angle_offset_deg = sm['liveParameters'].angleOffset steering_wheel_angle_deg = sm['carState'].steeringAngle # Update vehicle model x = max(sm['liveParameters'].stiffnessFactor, 0.1) sr = max(sm['liveParameters'].steerRatio, 0.1) VM.update_params(x, sr) curvature_factor = VM.curvature_factor(v_ego) measured_curvature = -curvature_factor * math.radians( steering_wheel_angle_deg - steering_wheel_angle_offset_deg) / VM.sR md = sm['modelV2'] self.LP.parse_model(sm['modelV2']) if len(md.position.x) == TRAJECTORY_SIZE and len( md.orientation.x) == TRAJECTORY_SIZE: self.path_xyz = np.column_stack( [md.position.x, md.position.y, md.position.z]) self.t_idxs = np.array(md.position.t) self.plan_yaw = list(md.orientation.z) # Lane change logic one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN if sm['carState'].leftBlinker: self.lane_change_direction = LaneChangeDirection.left elif sm['carState'].rightBlinker: self.lane_change_direction = LaneChangeDirection.right if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or ( not self.lane_change_enabled): self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none else: torque_applied = sm['carState'].steeringPressed and \ ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or (sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) blindspot_detected = ( (sm['carState'].leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or (sm['carState'].rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob # State transitions # off if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: self.lane_change_state = LaneChangeState.preLaneChange self.lane_change_ll_prob = 1.0 # pre elif self.lane_change_state == LaneChangeState.preLaneChange: if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off elif torque_applied and not blindspot_detected: self.lane_change_state = LaneChangeState.laneChangeStarting # starting elif self.lane_change_state == LaneChangeState.laneChangeStarting: # fade out over .5s self.lane_change_ll_prob = max( self.lane_change_ll_prob - 2 * DT_MDL, 0.0) # 98% certainty if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01: self.lane_change_state = LaneChangeState.laneChangeFinishing # finishing elif self.lane_change_state == LaneChangeState.laneChangeFinishing: # fade in laneline over 1s self.lane_change_ll_prob = min( self.lane_change_ll_prob + DT_MDL, 1.0) if one_blinker and self.lane_change_ll_prob > 0.99: self.lane_change_state = LaneChangeState.preLaneChange elif self.lane_change_ll_prob > 0.99: self.lane_change_state = LaneChangeState.off if self.lane_change_state in [ LaneChangeState.off, LaneChangeState.preLaneChange ]: self.lane_change_timer = 0.0 else: self.lane_change_timer += DT_MDL self.prev_one_blinker = one_blinker self.desire = DESIRES[self.lane_change_direction][ self.lane_change_state] # Turn off lanes during lane change if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft: self.LP.lll_prob *= self.lane_change_ll_prob self.LP.rll_prob *= self.lane_change_ll_prob d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz) y_pts = np.interp(v_ego * self.t_idxs[:MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1]) heading_pts = np.interp(v_ego * self.t_idxs[:MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw) self.y_pts = y_pts assert len(y_pts) == MPC_N + 1 assert len(heading_pts) == MPC_N + 1 self.libmpc.run_mpc(self.cur_state, self.mpc_solution, float(v_ego), CAR_ROTATION_RADIUS, list(y_pts), list(heading_pts)) # init state for next self.cur_state.x = 0.0 self.cur_state.y = 0.0 self.cur_state.psi = 0.0 self.cur_state.curvature = interp(DT_MDL, self.t_idxs[:MPC_N + 1], self.mpc_solution.curvature) # TODO this needs more thought, use .2s extra for now to estimate other delays delay = CP.steerActuatorDelay + .2 next_curvature = interp(delay, self.t_idxs[:MPC_N + 1], self.mpc_solution.curvature) psi = interp(delay, self.t_idxs[:MPC_N + 1], self.mpc_solution.psi) next_curvature_rate = self.mpc_solution.curvature_rate[0] next_curvature_from_psi = psi / (v_ego * delay) if psi > self.mpc_solution.curvature[0] * delay * v_ego: next_curvature = max(next_curvature_from_psi, next_curvature) else: next_curvature = min(next_curvature_from_psi, next_curvature) # reset to current steer angle if not active or overriding if active: curvature_desired = next_curvature desired_curvature_rate = next_curvature_rate else: curvature_desired = measured_curvature desired_curvature_rate = 0.0 # negative sign, controls uses different convention self.desired_steering_wheel_angle_deg = -float( math.degrees(curvature_desired * VM.sR) / curvature_factor) + steering_wheel_angle_offset_deg self.desired_steering_wheel_angle_rate_deg = -float( math.degrees(desired_curvature_rate * VM.sR) / curvature_factor) # Check for infeasable MPC solution mpc_nans = any(math.isnan(x) for x in self.mpc_solution.curvature) t = sec_since_boot() if mpc_nans: self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, CP.steerRateCost) self.cur_state.curvature = measured_curvature if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning("Lateral mpc - nan: True") if self.mpc_solution[ 0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge self.solution_invalid_cnt += 1 else: self.solution_invalid_cnt = 0
def update(self, pm, CS, lead): v_ego = CS.vEgo # Setup current mpc state self.cur_state[0].x_ego = 0.0 if lead is not None and lead.status: x_lead = lead.dRel v_lead = max(0.0, lead.vLead) a_lead = lead.aLeadK if (v_lead < 0.1 or -a_lead / 2.0 > v_lead): v_lead = 0.0 a_lead = 0.0 self.a_lead_tau = lead.aLeadTau self.new_lead = False if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5: self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead, a_lead, self.a_lead_tau) self.new_lead = True self.prev_lead_status = True self.prev_lead_x = x_lead self.cur_state[0].x_l = x_lead self.cur_state[0].v_l = v_lead else: self.prev_lead_status = False # Fake a fast lead car, so mpc keeps running self.cur_state[0].x_l = 50.0 self.cur_state[0].v_l = v_ego + 10.0 a_lead = 0.0 self.a_lead_tau = _LEAD_ACCEL_TAU # Calculate mpc t = sec_since_boot() # scc smoother cruise_gap = int(clip(CS.cruiseGap, 1., 4.)) TR = interp(float(cruise_gap), [1., 2., 3., 4.], [1.0, 1.3, 1.7, 2.3]) if self.cruise_gap != cruise_gap: self.cruise_gap = cruise_gap self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, TR) duration = int((sec_since_boot() - t) * 1e9) if LOG_MPC: self.send_mpc_solution(pm, n_its, duration) # Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed self.v_mpc = self.mpc_solution[0].v_ego[1] self.a_mpc = self.mpc_solution[0].a_ego[1] self.v_mpc_future = self.mpc_solution[0].v_ego[10] # Reset if NaN or goes through lead car crashing = any(lead - ego < -50 for ( lead, ego) in zip(self.mpc_solution[0].x_l, self.mpc_solution[0].x_ego)) nans = any(math.isnan(x) for x in self.mpc_solution[0].v_ego) backwards = min(self.mpc_solution[0].v_ego) < -0.01 if ((backwards or crashing) and self.prev_lead_status) or nans: if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning( "Longitudinal mpc %d reset - backwards: %s crashing: %s nan: %s" % (self.mpc_id, backwards, crashing, nans)) self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) self.cur_state[0].v_ego = v_ego self.cur_state[0].a_ego = 0.0 self.v_mpc = v_ego self.a_mpc = CS.aEgo self.prev_lead_status = False
def thermald_thread(end_event, hw_queue): pm = messaging.PubMaster(['deviceState']) sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "controlsState", "pandaStates"], poll=["pandaStates"]) fan_speed = 0 count = 0 onroad_conditions: Dict[str, bool] = { "ignition": False, } startup_conditions: Dict[str, bool] = {} startup_conditions_prev: Dict[str, bool] = {} off_ts = None started_ts = None started_seen = False thermal_status = ThermalStatus.green usb_power = True last_hw_state = HardwareState( network_type=NetworkType.none, network_strength=NetworkStrength.unknown, network_info=None, nvme_temps=[], modem_temps=[], ) current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML) temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML) should_start_prev = False in_car = False handle_fan = None is_uno = False engaged_prev = False params = Params() power_monitor = PowerMonitoring() HARDWARE.initialize_hardware() thermal_config = HARDWARE.get_thermal_config() # TODO: use PI controller for UNO controller = PIController(k_p=0, k_i=2e-3, k_f=1, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML)) while not end_event.is_set(): sm.update(PANDA_STATES_TIMEOUT) pandaStates = sm['pandaStates'] peripheralState = sm['peripheralState'] msg = read_thermal(thermal_config) if sm.updated['pandaStates'] and len(pandaStates) > 0: # Set ignition based on any panda connected onroad_conditions["ignition"] = any(ps.ignitionLine or ps.ignitionCan for ps in pandaStates if ps.pandaType != log.PandaState.PandaType.unknown) pandaState = pandaStates[0] in_car = pandaState.harnessStatus != log.PandaState.HarnessStatus.notConnected usb_power = peripheralState.usbPowerMode != log.PeripheralState.UsbPowerMode.client # Setup fan handler on first connect to panda if handle_fan is None and peripheralState.pandaType != log.PandaState.PandaType.unknown: is_uno = peripheralState.pandaType == log.PandaState.PandaType.uno if TICI: cloudlog.info("Setting up TICI fan handler") handle_fan = handle_fan_tici elif is_uno or PC: cloudlog.info("Setting up UNO fan handler") handle_fan = handle_fan_uno else: cloudlog.info("Setting up EON fan handler") setup_eon_fan() handle_fan = handle_fan_eon try: last_hw_state = hw_queue.get_nowait() except queue.Empty: pass msg.deviceState.freeSpacePercent = get_available_percent(default=100.0) msg.deviceState.memoryUsagePercent = int(round(psutil.virtual_memory().percent)) msg.deviceState.cpuUsagePercent = [int(round(n)) for n in psutil.cpu_percent(percpu=True)] msg.deviceState.gpuUsagePercent = int(round(HARDWARE.get_gpu_usage_percent())) msg.deviceState.networkType = last_hw_state.network_type msg.deviceState.networkStrength = last_hw_state.network_strength if last_hw_state.network_info is not None: msg.deviceState.networkInfo = last_hw_state.network_info msg.deviceState.nvmeTempC = last_hw_state.nvme_temps msg.deviceState.modemTempC = last_hw_state.modem_temps msg.deviceState.screenBrightnessPercent = HARDWARE.get_screen_brightness() msg.deviceState.batteryPercent = HARDWARE.get_battery_capacity() msg.deviceState.batteryCurrent = HARDWARE.get_battery_current() msg.deviceState.usbOnline = HARDWARE.get_usb_present() current_filter.update(msg.deviceState.batteryCurrent / 1e6) max_comp_temp = temp_filter.update( max(max(msg.deviceState.cpuTempC), msg.deviceState.memoryTempC, max(msg.deviceState.gpuTempC)) ) if handle_fan is not None: fan_speed = handle_fan(controller, max_comp_temp, fan_speed, onroad_conditions["ignition"]) msg.deviceState.fanSpeedPercentDesired = fan_speed is_offroad_for_5_min = (started_ts is None) and ((not started_seen) or (off_ts is None) or (sec_since_boot() - off_ts > 60 * 5)) if is_offroad_for_5_min and max_comp_temp > OFFROAD_DANGER_TEMP: # If device is offroad we want to cool down before going onroad # since going onroad increases load and can make temps go over 107 thermal_status = ThermalStatus.danger else: current_band = THERMAL_BANDS[thermal_status] band_idx = list(THERMAL_BANDS.keys()).index(thermal_status) if current_band.min_temp is not None and max_comp_temp < current_band.min_temp: thermal_status = list(THERMAL_BANDS.keys())[band_idx - 1] elif current_band.max_temp is not None and max_comp_temp > current_band.max_temp: thermal_status = list(THERMAL_BANDS.keys())[band_idx + 1] # **** starting logic **** # Ensure date/time are valid now = datetime.datetime.utcnow() startup_conditions["time_valid"] = (now.year > 2020) or (now.year == 2020 and now.month >= 10) set_offroad_alert_if_changed("Offroad_InvalidTime", (not startup_conditions["time_valid"])) startup_conditions["up_to_date"] = params.get("Offroad_ConnectivityNeeded") is None or params.get_bool("DisableUpdates") or params.get_bool("SnoozeUpdate") startup_conditions["not_uninstalling"] = not params.get_bool("DoUninstall") startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version # with 2% left, we killall, otherwise the phone will take a long time to boot startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2 startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version or \ params.get_bool("Passive") startup_conditions["not_driver_view"] = not params.get_bool("IsDriverViewEnabled") startup_conditions["not_taking_snapshot"] = not params.get_bool("IsTakingSnapshot") # if any CPU gets above 107 or the battery gets above 63, kill all processes # controls will warn with CPU above 95 or battery above 60 onroad_conditions["device_temp_good"] = thermal_status < ThermalStatus.danger set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", (not onroad_conditions["device_temp_good"])) if TICI: missing = (not Path("/data/media").is_mount()) and (not os.path.isfile("/persist/comma/living-in-the-moment")) set_offroad_alert_if_changed("Offroad_StorageMissing", missing) # Handle offroad/onroad transition should_start = all(onroad_conditions.values()) if started_ts is None: should_start = should_start and all(startup_conditions.values()) if should_start != should_start_prev or (count == 0): params.put_bool("IsOnroad", should_start) params.put_bool("IsOffroad", not should_start) params.put_bool("IsEngaged", False) engaged_prev = False HARDWARE.set_power_save(not should_start) if sm.updated['controlsState']: engaged = sm['controlsState'].enabled if engaged != engaged_prev: params.put_bool("IsEngaged", engaged) engaged_prev = engaged try: with open('/dev/kmsg', 'w') as kmsg: kmsg.write(f"<3>[thermald] engaged: {engaged}\n") except Exception: pass if should_start: off_ts = None if started_ts is None: started_ts = sec_since_boot() started_seen = True else: if onroad_conditions["ignition"] and (startup_conditions != startup_conditions_prev): cloudlog.event("Startup blocked", startup_conditions=startup_conditions, onroad_conditions=onroad_conditions) started_ts = None if off_ts is None: off_ts = sec_since_boot() # Offroad power monitoring power_monitor.calculate(peripheralState, onroad_conditions["ignition"]) msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used() msg.deviceState.carBatteryCapacityUwh = max(0, power_monitor.get_car_battery_capacity()) current_power_draw = HARDWARE.get_current_power_draw() # pylint: disable=assignment-from-none msg.deviceState.powerDrawW = current_power_draw if current_power_draw is not None else 0 # Check if we need to disable charging (handled by boardd) msg.deviceState.chargingDisabled = power_monitor.should_disable_charging(onroad_conditions["ignition"], in_car, off_ts) # Check if we need to shut down if power_monitor.should_shutdown(peripheralState, onroad_conditions["ignition"], in_car, off_ts, started_seen): cloudlog.warning(f"shutting device down, offroad since {off_ts}") params.put_bool("DoShutdown", True) msg.deviceState.chargingError = current_filter.x > 0. and msg.deviceState.batteryPercent < 90 # if current is positive, then battery is being discharged msg.deviceState.started = started_ts is not None msg.deviceState.startedMonoTime = int(1e9*(started_ts or 0)) last_ping = params.get("LastAthenaPingTime") if last_ping is not None: msg.deviceState.lastAthenaPingTime = int(last_ping) msg.deviceState.thermalStatus = thermal_status pm.send("deviceState", msg) if EON and not is_uno: set_offroad_alert_if_changed("Offroad_ChargeDisabled", (not usb_power)) should_start_prev = should_start startup_conditions_prev = startup_conditions.copy() # Log to statsd statlog.gauge("free_space_percent", msg.deviceState.freeSpacePercent) statlog.gauge("gpu_usage_percent", msg.deviceState.gpuUsagePercent) statlog.gauge("memory_usage_percent", msg.deviceState.memoryUsagePercent) for i, usage in enumerate(msg.deviceState.cpuUsagePercent): statlog.gauge(f"cpu{i}_usage_percent", usage) for i, temp in enumerate(msg.deviceState.cpuTempC): statlog.gauge(f"cpu{i}_temperature", temp) for i, temp in enumerate(msg.deviceState.gpuTempC): statlog.gauge(f"gpu{i}_temperature", temp) statlog.gauge("memory_temperature", msg.deviceState.memoryTempC) statlog.gauge("ambient_temperature", msg.deviceState.ambientTempC) for i, temp in enumerate(msg.deviceState.pmicTempC): statlog.gauge(f"pmic{i}_temperature", temp) for i, temp in enumerate(last_hw_state.nvme_temps): statlog.gauge(f"nvme_temperature{i}", temp) for i, temp in enumerate(last_hw_state.modem_temps): statlog.gauge(f"modem_temperature{i}", temp) statlog.gauge("fan_speed_percent_desired", msg.deviceState.fanSpeedPercentDesired) statlog.gauge("screen_brightness_percent", msg.deviceState.screenBrightnessPercent) # report to server once every 10 minutes if (count % int(600. / DT_TRML)) == 0: if EON and started_ts is None and msg.deviceState.memoryUsagePercent > 40: cloudlog.event("High offroad memory usage", mem=msg.deviceState.memoryUsagePercent) cloudlog.event("STATUS_PACKET", count=count, pandaStates=[strip_deprecated_keys(p.to_dict()) for p in pandaStates], peripheralState=strip_deprecated_keys(peripheralState.to_dict()), location=(strip_deprecated_keys(sm["gpsLocationExternal"].to_dict()) if sm.alive["gpsLocationExternal"] else None), deviceState=strip_deprecated_keys(msg.to_dict())) count += 1
def update(self, sm, pm, CP, VM): v_ego = sm['carState'].vEgo angle_steers = sm['carState'].steeringAngle active = sm['controlsState'].active angle_offset = sm['liveParameters'].angleOffset # Run MPC self.angle_steers_des_prev = self.angle_steers_des_mpc # Update vehicle model x = max(sm['liveParameters'].stiffnessFactor, 0.1) sr = max(sm['liveParameters'].steerRatio, 0.1) VM.update_params(x, sr) curvature_factor = VM.curvature_factor(v_ego) self.LP.parse_model(sm['model']) # Lane change logic one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN if sm['carState'].leftBlinker: self.lane_change_direction = LaneChangeDirection.left elif sm['carState'].rightBlinker: self.lane_change_direction = LaneChangeDirection.right if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or ( not one_blinker) or (not self.lane_change_enabled): self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none else: torque_applied = sm['carState'].steeringPressed and \ ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or (sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob # State transitions # off if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: self.lane_change_state = LaneChangeState.preLaneChange self.lane_change_ll_prob = 1.0 # pre elif self.lane_change_state == LaneChangeState.preLaneChange: if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off elif torque_applied: self.lane_change_state = LaneChangeState.laneChangeStarting # starting elif self.lane_change_state == LaneChangeState.laneChangeStarting: # fade out over .5s self.lane_change_ll_prob = max( self.lane_change_ll_prob - 2 * DT_MDL, 0.0) # 98% certainty if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01: self.lane_change_state = LaneChangeState.laneChangeFinishing # finishing elif self.lane_change_state == LaneChangeState.laneChangeFinishing: # fade in laneline over 1s self.lane_change_ll_prob = min( self.lane_change_ll_prob + DT_MDL, 1.0) if one_blinker and self.lane_change_ll_prob > 0.99: self.lane_change_state = LaneChangeState.preLaneChange elif self.lane_change_ll_prob > 0.99: self.lane_change_state = LaneChangeState.off if self.lane_change_state in [ LaneChangeState.off, LaneChangeState.preLaneChange ]: self.lane_change_timer = 0.0 else: self.lane_change_timer += DT_MDL self.prev_one_blinker = one_blinker desire = DESIRES[self.lane_change_direction][self.lane_change_state] # Turn off lanes during lane change if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft: self.LP.l_prob *= self.lane_change_ll_prob self.LP.r_prob *= self.lane_change_ll_prob self.LP.update_d_poly(v_ego) # account for actuation delay self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, CP.steerActuatorDelay) v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed self.libmpc.run_mpc(self.cur_state, self.mpc_solution, list(self.LP.l_poly), list(self.LP.r_poly), list(self.LP.d_poly), self.LP.l_prob, self.LP.r_prob, curvature_factor, v_ego_mpc, self.LP.lane_width) # reset to current steer angle if not active or overriding if active: delta_desired = self.mpc_solution[0].delta[1] rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR) else: delta_desired = math.radians(angle_steers - angle_offset) / VM.sR rate_desired = 0.0 self.cur_state[0].delta = delta_desired self.angle_steers_des_mpc = float( math.degrees(delta_desired * VM.sR) + angle_offset) # Check for infeasable MPC solution mpc_nans = any(math.isnan(x) for x in self.mpc_solution[0].delta) t = sec_since_boot() if mpc_nans: self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost) self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning("Lateral mpc - nan: True") if self.mpc_solution[ 0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge self.solution_invalid_cnt += 1 else: self.solution_invalid_cnt = 0 plan_solution_valid = self.solution_invalid_cnt < 2 plan_send = messaging.new_message('pathPlan') plan_send.valid = sm.all_alive_and_valid(service_list=[ 'carState', 'controlsState', 'liveParameters', 'model' ]) plan_send.pathPlan.laneWidth = float(self.LP.lane_width) plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly] plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly] plan_send.pathPlan.lProb = float(self.LP.l_prob) plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly] plan_send.pathPlan.rProb = float(self.LP.r_prob) plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc) plan_send.pathPlan.rateSteers = float(rate_desired) plan_send.pathPlan.angleOffset = float( sm['liveParameters'].angleOffsetAverage) plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid) plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid) plan_send.pathPlan.desire = desire plan_send.pathPlan.laneChangeState = self.lane_change_state plan_send.pathPlan.laneChangeDirection = self.lane_change_direction pm.send('pathPlan', plan_send) if LOG_MPC: dat = messaging.new_message('liveMpc') dat.liveMpc.x = list(self.mpc_solution[0].x) dat.liveMpc.y = list(self.mpc_solution[0].y) dat.liveMpc.psi = list(self.mpc_solution[0].psi) dat.liveMpc.delta = list(self.mpc_solution[0].delta) dat.liveMpc.cost = self.mpc_solution[0].cost pm.send('liveMpc', dat)
def update(self, CS, lead, v_cruise_setpoint): # Setup current mpc state self.cur_state[0].x_ego = 0.0 if lead is not None and lead.status: x_lead = lead.dRel v_lead = max(0.0, lead.vLead) a_lead = lead.aLeadK if (v_lead < 0.1 or -a_lead / 2.0 > v_lead): v_lead = 0.0 a_lead = 0.0 self.a_lead_tau = max(lead.aLeadTau, (a_lead**2 * math.pi) / (2 * (v_lead + 0.01)**2)) self.new_lead = False if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5: self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead, a_lead, self.a_lead_tau) self.new_lead = True self.prev_lead_status = True self.prev_lead_x = x_lead self.cur_state[0].x_l = x_lead self.cur_state[0].v_l = v_lead else: self.prev_lead_status = False # Fake a fast lead car, so mpc keeps running self.cur_state[0].x_l = 50.0 self.cur_state[0].v_l = CS.vEgo + 10.0 a_lead = 0.0 self.a_lead_tau = _LEAD_ACCEL_TAU # Calculate mpc t = sec_since_boot() n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead) duration = int((sec_since_boot() - t) * 1e9) self.send_mpc_solution(n_its, duration) # Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed self.v_mpc = self.mpc_solution[0].v_ego[1] self.a_mpc = self.mpc_solution[0].a_ego[1] self.v_mpc_future = self.mpc_solution[0].v_ego[10] # Reset if NaN or goes through lead car dls = np.array(list(self.mpc_solution[0].x_l)) - np.array(list(self.mpc_solution[0].x_ego)) crashing = min(dls) < -50.0 nans = np.any(np.isnan(list(self.mpc_solution[0].v_ego))) backwards = min(list(self.mpc_solution[0].v_ego)) < -0.01 if ((backwards or crashing) and self.prev_lead_status) or nans: if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning("Longitudinal mpc %d reset - backwards: %s crashing: %s nan: %s" % ( self.mpc_id, backwards, crashing, nans)) self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) self.cur_state[0].v_ego = CS.vEgo self.cur_state[0].a_ego = 0.0 self.v_mpc = CS.vEgo self.a_mpc = CS.aEgo self.prev_lead_status = False