class PCCController(): def __init__(self, carcontroller): self.CC = carcontroller self.human_cruise_action_time = 0 self.pcc_available = self.prev_pcc_available = False self.pedal_timeout_frame = 0 self.accelerator_pedal_pressed = self.prev_accelerator_pedal_pressed = False self.automated_cruise_action_time = 0 self.last_angle = 0. self.radarState = messaging.sub_sock('radarState', conflate=True) self.live_map_data = messaging.sub_sock('liveMapData', conflate=True) self.lead_1 = None self.last_update_time = 0 self.enable_pedal_cruise = False self.stalk_pull_time_ms = 0 self.prev_stalk_pull_time_ms = -1000 self.prev_pcm_acc_status = 0 self.prev_cruise_buttons = CruiseButtons.IDLE self.pedal_speed_kph = 0. self.speed_limit_kph = 0. self.prev_speed_limit_kph = 0. self.pedal_idx = 0 self.pedal_steady = 0. self.prev_tesla_accel = 0. self.prev_tesla_pedal = 0. self.torqueLevel_last = 0. self.prev_v_ego = 0. self.PedalForZeroTorque = 18. #starting number, works on my S85 self.lastTorqueForPedalForZeroTorque = TORQUE_LEVEL_DECEL self.v_pid = 0. self.a_pid = 0. self.last_output_gb = 0. self.last_speed_kph = None #for smoothing the changes in speed self.v_acc_start = 0.0 self.a_acc_start = 0.0 self.v_acc = 0.0 self.v_acc_sol = 0.0 self.v_acc_future = 0.0 self.a_acc = 0.0 self.a_acc_sol = 0.0 self.v_cruise = 0.0 self.a_cruise = 0.0 #Long Control self.LoC = None #when was radar data last updated? self.lead_last_seen_time_ms = 0 self.continuous_lead_sightings = 0 self.params = Params() average_speed_over_x_suggestions = 6 # 0.3 seconds (20x a second) self.fleet_speed = FleetSpeed(average_speed_over_x_suggestions) def load_pid(self): try: v_pid_json = open(V_PID_FILE) data = json.load(v_pid_json) if (self.LoC): if self.LoC.pid: self.LoC.pid.p = data['p'] self.LoC.pid.i = data['i'] self.LoC.pid.f = data['f'] else: print("self.LoC not initialized!") except: print("file not present, creating at next reset") #Helper function for saving the PCC pid constants across drives def save_pid(self, pid): data = {} data['p'] = pid.p data['i'] = pid.i data['f'] = pid.f try: with open(V_PID_FILE, 'w') as outfile: json.dump(data, outfile) except IOError: print("PDD pid parameters could not be saved to file") def reset(self, v_pid): if self.LoC and RESET_PID_ON_DISENGAGE: self.LoC.reset(v_pid) def update_stat(self, CS, frame): if not self.LoC: self.LoC = LongControl(CS.CP, tesla_compute_gb) # Get v_id from the stored file when initiating the LoC and reset_on_disengage==false if (not RESET_PID_ON_DISENGAGE): self.load_pid() self._update_pedal_state(CS, frame) can_sends = [] if not self.pcc_available: timed_out = frame >= self.pedal_timeout_frame if timed_out or CS.pedal_interceptor_state > 0: if self.prev_pcc_available: CS.UE.custom_alert_message( 4, "Pedal Interceptor %s" % ("timed out" if timed_out else "fault (state %s)" % CS.pedal_interceptor_state), 200, 4) if frame % 50 == 0: # send reset command idx = self.pedal_idx self.pedal_idx = (self.pedal_idx + 1) % 16 can_sends.append( teslacan.create_pedal_command_msg(0, 0, idx)) return can_sends prev_enable_pedal_cruise = self.enable_pedal_cruise # disable on brake if CS.brake_pressed and self.enable_pedal_cruise: self.enable_pedal_cruise = False self.reset(0.) # process any stalk movement curr_time_ms = _current_time_millis() speed_uom_kph = 1. if CS.imperial_speed_units: speed_uom_kph = CV.MPH_TO_KPH if (CS.cruise_buttons == CruiseButtons.MAIN and self.prev_cruise_buttons != CruiseButtons.MAIN): self.prev_stalk_pull_time_ms = self.stalk_pull_time_ms self.stalk_pull_time_ms = curr_time_ms double_pull = self.stalk_pull_time_ms - self.prev_stalk_pull_time_ms < STALK_DOUBLE_PULL_MS ready = (CS.cstm_btns.get_button_status(PCCModes.BUTTON_NAME) > PCCState.OFF and (CruiseState.is_off(CS.pcm_acc_status)) or CS.forcePedalOverCC) if ready and double_pull: # A double pull enables ACC. updating the max ACC speed if necessary. self.enable_pedal_cruise = True self.reset(CS.v_ego) # Increase PCC speed to match current, if applicable. # We round the target speed in the user's units of measurement to avoid jumpy speed readings current_speed_kph_uom_rounded = int(CS.v_ego * CV.MS_TO_KPH / speed_uom_kph + 0.5) * speed_uom_kph self.pedal_speed_kph = max(current_speed_kph_uom_rounded, self.speed_limit_kph) # Handle pressing the cancel button. elif CS.cruise_buttons == CruiseButtons.CANCEL: self.enable_pedal_cruise = False self.pedal_speed_kph = 0. self.stalk_pull_time_ms = 0 self.prev_stalk_pull_time_ms = -1000 # Handle pressing up and down buttons. elif (self.enable_pedal_cruise and CS.cruise_buttons != self.prev_cruise_buttons): # Real stalk command while PCC is already enabled. Adjust the max PCC speed if necessary. # We round the target speed in the user's units of measurement to avoid jumpy speed readings actual_speed_kph_uom_rounded = int( CS.v_ego * CV.MS_TO_KPH / speed_uom_kph + 0.5) * speed_uom_kph if CS.cruise_buttons == CruiseButtons.RES_ACCEL: self.pedal_speed_kph = max( self.pedal_speed_kph, actual_speed_kph_uom_rounded) + speed_uom_kph elif CS.cruise_buttons == CruiseButtons.RES_ACCEL_2ND: self.pedal_speed_kph = max( self.pedal_speed_kph, actual_speed_kph_uom_rounded) + 5 * speed_uom_kph elif CS.cruise_buttons == CruiseButtons.DECEL_SET: self.pedal_speed_kph = self.pedal_speed_kph - speed_uom_kph elif CS.cruise_buttons == CruiseButtons.DECEL_2ND: self.pedal_speed_kph = self.pedal_speed_kph - 5 * speed_uom_kph # Clip PCC speed between 0 and 170 KPH. self.pedal_speed_kph = clip(self.pedal_speed_kph, MIN_PCC_V_KPH, MAX_PCC_V_KPH) # If something disabled cruise control, disable PCC too elif self.enable_pedal_cruise and CS.pcm_acc_status and not CS.forcePedalOverCC: self.enable_pedal_cruise = False # A single pull disables PCC (falling back to just steering). Wait some time # in case a double pull comes along. elif (self.enable_pedal_cruise and curr_time_ms - self.stalk_pull_time_ms > STALK_DOUBLE_PULL_MS and self.stalk_pull_time_ms - self.prev_stalk_pull_time_ms > STALK_DOUBLE_PULL_MS): self.enable_pedal_cruise = False # Notify if PCC was toggled if prev_enable_pedal_cruise and not self.enable_pedal_cruise: CS.UE.custom_alert_message(3, "PCC Disabled", 150, 4) CS.cstm_btns.set_button_status(PCCModes.BUTTON_NAME, PCCState.STANDBY) self.fleet_speed.reset_averager() #save the pid parameters to params file self.save_pid(self.LoC.pid) elif self.enable_pedal_cruise and not prev_enable_pedal_cruise: CS.UE.custom_alert_message(2, "PCC Enabled", 150) CS.cstm_btns.set_button_status(PCCModes.BUTTON_NAME, PCCState.ENABLED) # Update the UI to show whether the current car state allows PCC. if CS.cstm_btns.get_button_status(PCCModes.BUTTON_NAME) in [ PCCState.STANDBY, PCCState.NOT_READY ]: if CruiseState.is_off(CS.pcm_acc_status) or CS.forcePedalOverCC: CS.cstm_btns.set_button_status(PCCModes.BUTTON_NAME, PCCState.STANDBY) else: CS.cstm_btns.set_button_status(PCCModes.BUTTON_NAME, PCCState.NOT_READY) # Update prev state after all other actions. self.prev_cruise_buttons = CS.cruise_buttons self.prev_pcm_acc_status = CS.pcm_acc_status return can_sends def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, set_speed_limit_active, speed_limit_offset, alca_enabled): idx = self.pedal_idx self.prev_speed_limit_kph = self.speed_limit_kph ###################################################################################### # Determine pedal "zero" # #save position for cruising (zero acc, zero brake, no torque) when we are above 10 MPH ###################################################################################### if (CS.torqueLevel < TORQUE_LEVEL_ACC and CS.torqueLevel > TORQUE_LEVEL_DECEL and CS.v_ego >= 10. * CV.MPH_TO_MS and abs(CS.torqueLevel) < abs(self.lastTorqueForPedalForZeroTorque) and self.prev_tesla_accel > 0.): self.PedalForZeroTorque = self.prev_tesla_accel self.lastTorqueForPedalForZeroTorque = CS.torqueLevel #print ("Detected new Pedal For Zero Torque at %s" % (self.PedalForZeroTorque)) #print ("Torque level at detection %s" % (CS.torqueLevel)) #print ("Speed level at detection %s" % (CS.v_ego * CV.MS_TO_MPH)) if set_speed_limit_active and speed_limit_ms > 0: self.speed_limit_kph = (speed_limit_ms + speed_limit_offset) * CV.MS_TO_KPH if int(self.prev_speed_limit_kph) != int(self.speed_limit_kph): self.pedal_speed_kph = self.speed_limit_kph # reset MovingAverage for fleet speed when speed limit changes self.fleet_speed.reset_averager() else: # reset internal speed limit, so double pull doesn't set higher speed than current (e.g. after leaving the highway) self.speed_limit_kph = 0. self.pedal_idx = (self.pedal_idx + 1) % 16 if not self.pcc_available or not enabled: return 0., 0, idx # Alternative speed decision logic that uses the lead car's distance # and speed more directly. # Bring in the lead car distance from the radarState feed radSt = messaging.recv_one_or_none(self.radarState) mapd = messaging.recv_one_or_none(self.live_map_data) if radSt is not None: self.lead_1 = radSt.radarState.leadOne if _is_present(self.lead_1): self.lead_last_seen_time_ms = _current_time_millis() self.continuous_lead_sightings += 1 else: self.continuous_lead_sightings = 0 v_ego = CS.v_ego following = self.lead_1.status and self.lead_1.dRel < MAX_RADAR_DISTANCE and self.lead_1.vLeadK > v_ego and self.lead_1.aLeadK > 0.0 accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego)] accel_limits[1] *= _accel_limit_multiplier(CS, self.lead_1) accel_limits[0] = _decel_limit(accel_limits[0], CS.v_ego, self.lead_1, CS, self.pedal_speed_kph) jerk_limits = [ min(-0.1, accel_limits[0] / 2.), max(0.1, accel_limits[1] / 2.) ] # TODO: make a separate lookup for jerk tuning #accel_limits = limit_accel_in_turns(v_ego, CS.angle_steers, accel_limits, CS.CP) output_gb = 0 #################################################################### # this mode (Follow) uses the Follow logic created by JJ for ACC # # once the speed is detected we have to use our own PID to determine # how much accel and break we have to do #################################################################### if PCCModes.is_selected(FollowMode(), CS.cstm_btns): enabled = self.enable_pedal_cruise and self.LoC.long_control_state in [ LongCtrlState.pid, LongCtrlState.stopping ] # determine if pedal is pressed by human self.prev_accelerator_pedal_pressed = self.accelerator_pedal_pressed self.accelerator_pedal_pressed = CS.pedal_interceptor_value > 10 #reset PID if we just lifted foot of accelerator if (not self.accelerator_pedal_pressed ) and self.prev_accelerator_pedal_pressed: self.reset(CS.v_ego) if self.enable_pedal_cruise and not self.accelerator_pedal_pressed: self.v_pid = self.calc_follow_speed_ms(CS, alca_enabled) if mapd is not None: v_curve = max_v_in_mapped_curve_ms(mapd.liveMapData, self.pedal_speed_kph) if v_curve: self.v_pid = min(self.v_pid, v_curve) # take fleet speed into consideration self.v_pid = min( self.v_pid, self.fleet_speed.adjust( CS, self.pedal_speed_kph * CV.KPH_TO_MS, frame)) # cruise speed can't be negative even if user is distracted self.v_pid = max(self.v_pid, 0.) jerk_min, jerk_max = _jerk_limits(CS.v_ego, self.lead_1, self.v_pid * CV.MS_TO_KPH, self.lead_last_seen_time_ms, CS) self.v_cruise, self.a_cruise = speed_smoother( self.v_acc_start, self.a_acc_start, self.v_pid, accel_limits[1], accel_limits[0], jerk_limits[1], jerk_limits[0], #jerk_max, jerk_min, _DT_MPC) # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) self.v_acc = self.v_cruise self.a_acc = self.a_cruise self.v_acc_future = self.v_pid # Interpolation of trajectory self.a_acc_sol = self.a_acc_start + (_DT / _DT_MPC) * ( self.a_acc - self.a_acc_start) self.v_acc_sol = self.v_acc_start + _DT * ( self.a_acc_sol + self.a_acc_start) / 2.0 self.v_acc_start = self.v_acc_sol self.a_acc_start = self.a_acc_sol # we will try to feed forward the pedal position.... we might want to feed the last_output_gb.... # op feeds forward self.a_acc_sol # it's all about testing now. vTarget = clip(self.v_acc_sol, 0, self.v_cruise) self.vTargetFuture = clip(self.v_acc_future, 0, self.v_pid) feedforward = self.a_acc_sol #feedforward = self.last_output_gb t_go, t_brake = self.LoC.update( self.enable_pedal_cruise, CS.v_ego, CS.brake_pressed != 0, CS.standstill, False, self.v_cruise, vTarget, self.vTargetFuture, feedforward, CS.CP) output_gb = t_go - t_brake #print ("Output GB Follow:", output_gb) else: self.reset(CS.v_ego) #print ("PID reset") output_gb = 0. starting = self.LoC.long_control_state == LongCtrlState.starting a_ego = min(CS.a_ego, 0.0) reset_speed = MIN_CAN_SPEED if starting else CS.v_ego reset_accel = CS.CP.startAccel if starting else a_ego self.v_acc = reset_speed self.a_acc = reset_accel self.v_acc_start = reset_speed self.a_acc_start = reset_accel self.v_cruise = reset_speed self.a_cruise = reset_accel self.v_acc_sol = reset_speed self.a_acc_sol = reset_accel self.v_pid = reset_speed self.last_speed_kph = None ############################################################## # This mode uses the longitudinal MPC built in OP # # we use the values from actuator.accel and actuator.brake ############################################################## elif PCCModes.is_selected(OpMode(), CS.cstm_btns): output_gb = actuators.gas - actuators.brake self.v_pid = -10. self.last_output_gb = output_gb # accel and brake apply_accel = clip( output_gb, 0., _accel_pedal_max(CS.v_ego, self.v_pid, self.lead_1, self.prev_tesla_accel, CS)) MPC_BRAKE_MULTIPLIER = 6. apply_brake = -clip( output_gb * MPC_BRAKE_MULTIPLIER, _brake_pedal_min(CS.v_ego, self.v_pid, self.lead_1, CS, self.pedal_speed_kph), 0.) # if speed is over 5mph, the "zero" is at PedalForZeroTorque; otherwise it is zero pedal_zero = 0. if CS.v_ego >= 5. * CV.MPH_TO_MS: pedal_zero = self.PedalForZeroTorque tesla_brake = clip((1. - apply_brake) * pedal_zero, 0, pedal_zero) tesla_accel = clip(apply_accel * (MAX_PEDAL_VALUE - pedal_zero), 0, MAX_PEDAL_VALUE - pedal_zero) tesla_pedal = tesla_brake + tesla_accel tesla_pedal = self.pedal_hysteresis(tesla_pedal, enabled) tesla_pedal = clip(tesla_pedal, self.prev_tesla_pedal - PEDAL_MAX_DOWN, self.prev_tesla_pedal + PEDAL_MAX_UP) tesla_pedal = clip(tesla_pedal, 0., MAX_PEDAL_VALUE) if self.enable_pedal_cruise else 0. enable_pedal = 1. if self.enable_pedal_cruise else 0. self.torqueLevel_last = CS.torqueLevel self.prev_tesla_pedal = tesla_pedal * enable_pedal self.prev_tesla_accel = apply_accel * enable_pedal self.prev_v_ego = CS.v_ego return self.prev_tesla_pedal, enable_pedal, idx # function to calculate the cruise speed based on a safe follow distance def calc_follow_speed_ms(self, CS, alca_enabled): # Make sure we were able to populate lead_1. if self.lead_1 is None: return None, None, None # dRel is in meters. lead_dist_m = self.lead_1.dRel if not CS.useTeslaRadar: lead_dist_m = _visual_radar_adjusted_dist_m(lead_dist_m, CS) # Grab the relative speed. v_rel = self.lead_1.vRel if abs(self.lead_1.vRel) > .5 else 0 a_rel = self.lead_1.aRel if abs(self.lead_1.aRel) > .5 else 0 rel_speed_kph = (v_rel + 0 * CS.apFollowTimeInS * a_rel) * CV.MS_TO_KPH # v_ego is in m/s, so safe_distance is in meters. safe_dist_m = _safe_distance_m(CS.v_ego, CS) # Current speed in kph actual_speed_kph = CS.v_ego * CV.MS_TO_KPH # speed and brake to issue if self.last_speed_kph is None: self.last_speed_kph = actual_speed_kph new_speed_kph = self.last_speed_kph ### Logic to determine best cruise speed ### if self.enable_pedal_cruise: # If no lead is present, accel up to max speed if lead_dist_m == 0 or lead_dist_m > MAX_RADAR_DISTANCE: new_speed_kph = self.pedal_speed_kph elif lead_dist_m > 0: #BB Use the Kalman lead speed and acceleration lead_absolute_speed_kph = actual_speed_kph + rel_speed_kph #(self.lead_1.vLeadK + _DT * self.lead_1.aLeadK) * CV.MS_TO_KPH rel_speed_kph = lead_absolute_speed_kph - actual_speed_kph if lead_dist_m < MIN_SAFE_DIST_M and rel_speed_kph >= 3: # If lead is going faster, but we're not at a safe distance, hold # speed and let the lead car move father away from us new_speed_kph = actual_speed_kph # If lead is not going faster than 3kpm from us, lets slow down a # bit to get him moving faster relative to us elif lead_dist_m < MIN_SAFE_DIST_M: new_speed_kph = MIN_PCC_V_KPH # In a 10 meter cruise zone, lets match the car in front elif safe_dist_m + 2 > lead_dist_m > MIN_SAFE_DIST_M: # BB we might want to try this and rel_speed_kph > 0: min_vrel_kph_map = OrderedDict([ # (distance in m, min allowed relative kph) (0.5 * safe_dist_m, 3.0), (0.8 * safe_dist_m, 2.0), (1.0 * safe_dist_m, 0.0) ]) min_vrel_kph = _interp_map(lead_dist_m, min_vrel_kph_map) new_speed_kph = lead_absolute_speed_kph - min_vrel_kph else: # Force speed into a band that is generally slower than lead if too # close, and faster than lead if too far. Allow a range of speeds at # any given distance, to prevent continuous jerky adjustments. # BB band should be % of v_ego min_vrel_kph_map = OrderedDict([ # (distance in m, min allowed relative kph) (0.5 * safe_dist_m, 3), (1.0 * safe_dist_m, -1 - 0.025 * CS.v_ego * CV.MS_TO_KPH), (1.5 * safe_dist_m, -5 - 0.05 * CS.v_ego * CV.MS_TO_KPH), (3.0 * safe_dist_m, -10 - 0.1 * CS.v_ego * CV.MS_TO_KPH) ]) min_vrel_kph = _interp_map(lead_dist_m, min_vrel_kph_map) max_vrel_kph_map = OrderedDict([ # (distance in m, max allowed relative kph) (0.5 * safe_dist_m, 6), (1.0 * safe_dist_m, 2), (1.5 * safe_dist_m, -3), # With visual radar the relative velocity is 0 until the confidence # gets high. So even a small negative number here gives constant # accel until lead lead car gets close enough to read. (3 * safe_dist_m, -7) ]) max_vrel_kph = _interp_map(lead_dist_m, max_vrel_kph_map) #if CS.useTeslaRadar: # min_vrel_kph = -1 # max_vrel_kph = -2 min_kph = lead_absolute_speed_kph - max_vrel_kph max_kph = lead_absolute_speed_kph - min_vrel_kph # In the special case were we are going faster than intended but it's # still an acceptable speed, accept it. This could happen if the # driver manually accelerates, or if we roll down a hill. In either # case, don't fight the extra velocity unless necessary. if lead_dist_m >= 0.8 * safe_dist_m and lead_absolute_speed_kph > actual_speed_kph and self.lead_1.aLeadK >= 0.: new_speed_kph = lead_absolute_speed_kph new_speed_kph = clip(new_speed_kph, min_kph, max_kph) if (actual_speed_kph > new_speed_kph) and ( min_kph < actual_speed_kph < max_kph) and (lead_absolute_speed_kph > 30): new_speed_kph = actual_speed_kph #BB disabled this condition as it might not allow fast enough braking #if (actual_speed_kph > 80) and abs(actual_speed_kph - new_speed_kph) < 3.: # new_speed_kph = (actual_speed_kph + new_speed_kph)/2.0 # Enforce limits on speed in the presence of a lead car. new_speed_kph = min( new_speed_kph, _max_safe_speed_kph(self.lead_1, CS), max( lead_absolute_speed_kph - _min_safe_vrel_kph( self.lead_1, CS, actual_speed_kph), 2)) # Enforce limits on speed new_speed_kph = clip(new_speed_kph, MIN_PCC_V_KPH, MAX_PCC_V_KPH) new_speed_kph = clip(new_speed_kph, MIN_PCC_V_KPH, self.pedal_speed_kph) if CS.turn_signal_blinking or (abs(CS.angle_steers) > ANGLE_STOP_ACCEL) or alca_enabled: # Don't accelerate during manual turns, curves or ALCA. new_speed_kph = min(new_speed_kph, self.last_speed_kph) #BB Last safety check. Zero if below MIN_SAFE_DIST_M if (MIN_SAFE_DIST_M > lead_dist_m > 0) and (rel_speed_kph < 3.): new_speed_kph = MIN_PCC_V_KPH self.last_speed_kph = new_speed_kph return new_speed_kph * CV.KPH_TO_MS def pedal_hysteresis(self, pedal, enabled): # for small accel oscillations within PEDAL_HYST_GAP, don't change the command if not enabled: # send 0 when disabled, otherwise acc faults self.pedal_steady = 0. elif pedal > self.pedal_steady + PEDAL_HYST_GAP: self.pedal_steady = pedal - PEDAL_HYST_GAP elif pedal < self.pedal_steady - PEDAL_HYST_GAP: self.pedal_steady = pedal + PEDAL_HYST_GAP return self.pedal_steady def _update_pedal_state(self, CS, frame): if CS.pedal_idx != CS.prev_pedal_idx: # time out pedal after 500ms without receiving a new CAN message from it self.pedal_timeout_frame = frame + 50 self.prev_pcc_available = self.pcc_available pedal_ready = frame < self.pedal_timeout_frame and CS.pedal_interceptor_state == 0 acc_disabled = CS.forcePedalOverCC or CruiseState.is_off( CS.pcm_acc_status) # Mark pedal unavailable while traditional cruise is on. self.pcc_available = pedal_ready and acc_disabled if self.pcc_available != self.prev_pcc_available: CS.config_ui_buttons(self.pcc_available, pedal_ready and not acc_disabled)
class ACCController(): # Tesla cruise only functions above 17 MPH MIN_CRUISE_SPEED_MS = 17.1 * CV.MPH_TO_MS def __init__(self, carcontroller): self.CC = carcontroller self.human_cruise_action_time = 0 self.automated_cruise_action_time = 0 self.radarState = messaging.sub_sock('radarState', conflate=True) self.last_update_time = 0 self.enable_adaptive_cruise = False self.prev_enable_adaptive_cruise = False # Whether to re-engage automatically after being paused due to low speed or # user-initated deceleration. self.autoresume = False self.adaptive = False self.last_brake_press_time = 0 self.last_cruise_stalk_pull_time = 0 self.prev_cruise_buttons = CruiseButtons.IDLE self.prev_pcm_acc_status = 0 self.acc_speed_kph = 0. self.speed_limit_kph = 0. self.prev_speed_limit_kph = 0. self.user_has_braked = False self.has_gone_below_min_speed = False self.fast_decel_time = 0 self.lead_last_seen_time_ms = 0 # BB speed for testing self.new_speed = 0 average_speed_over_x_suggestions = 20 # 1 second (20x a second) self.fleet_speed = FleetSpeed(average_speed_over_x_suggestions) # Updates the internal state of this controller based on user input, # specifically the steering wheel mounted cruise control stalk, and OpenPilot # UI buttons. def update_stat(self, CS, enabled): # Check if the cruise stalk was double pulled, indicating that adaptive # cruise control should be enabled. Twice in .75 seconds counts as a double # pull. self.prev_enable_adaptive_cruise = self.enable_adaptive_cruise acc_string = CS.cstm_btns.get_button_label2(ACCMode.BUTTON_NAME) acc_mode = ACCMode.from_label(acc_string) CS.cstm_btns.get_button( ACCMode.BUTTON_NAME).btn_label2 = acc_mode.label self.autoresume = acc_mode.autoresume self.adaptive = acc_mode.adaptive curr_time_ms = _current_time_millis() # Handle pressing the enable button. if (CS.cruise_buttons == CruiseButtons.MAIN or CS.cruise_buttons == CruiseButtons.DECEL_SET ) and self.prev_cruise_buttons != CS.cruise_buttons: double_pull = curr_time_ms - self.last_cruise_stalk_pull_time < 750 self.last_cruise_stalk_pull_time = curr_time_ms ready = (CS.cstm_btns.get_button_status( ACCMode.BUTTON_NAME) > ACCState.OFF and (enabled or (CS.cruise_buttons == CruiseButtons.DECEL_SET and not self.adaptive)) and CruiseState.is_enabled_or_standby(CS.pcm_acc_status) and CS.v_ego > self.MIN_CRUISE_SPEED_MS) if ready and double_pull and ( (self.adaptive and CS.cruise_buttons == CruiseButtons.MAIN) or ((not self.adaptive) and CS.cruise_buttons == CruiseButtons.DECEL_SET)): # A double pull enables ACC. updating the max ACC speed if necessary. self.enable_adaptive_cruise = True # Increase ACC speed to match current, if applicable. if self.adaptive: self.acc_speed_kph = max(CS.v_ego_raw * CV.MS_TO_KPH, self.speed_limit_kph) else: self.acc_speed_kph = CS.v_ego_raw * CV.MS_TO_KPH self.user_has_braked = False self.has_gone_below_min_speed = False else: # A single pull disables ACC (falling back to just steering). if CS.cruise_buttons == CruiseButtons.MAIN: self.enable_adaptive_cruise = False # Handle pressing the cancel button. if CS.cruise_buttons == CruiseButtons.CANCEL: self.enable_adaptive_cruise = False self.acc_speed_kph = 0. self.last_cruise_stalk_pull_time = 0 # Handle pressing up and down buttons. elif (CS.cruise_buttons != CruiseButtons.MAIN and self.enable_adaptive_cruise and CS.cruise_buttons != self.prev_cruise_buttons): self._update_max_acc_speed(CS) if CS.brake_pressed: self.user_has_braked = True self.last_brake_press_time = _current_time_millis() if not self.autoresume: self.enable_adaptive_cruise = False if CS.v_ego < self.MIN_CRUISE_SPEED_MS: self.has_gone_below_min_speed = True # If autoresume is not enabled and not in standard CC, disable if we hit the brakes or gone below 18mph if not self.autoresume: if (self.adaptive and not enabled ) or self.user_has_braked or self.has_gone_below_min_speed: self.enable_adaptive_cruise = False # Notify if ACC was toggled if self.prev_enable_adaptive_cruise and not self.enable_adaptive_cruise: CS.UE.custom_alert_message( 3, "%s Disabled" % ("ACC" if self.adaptive else "CC"), 150, 4) CS.cstm_btns.set_button_status(ACCMode.BUTTON_NAME, ACCState.STANDBY) self.fleet_speed.reset_averager() elif self.enable_adaptive_cruise: CS.cstm_btns.set_button_status(ACCMode.BUTTON_NAME, ACCState.ENABLED) if not self.prev_enable_adaptive_cruise: CS.UE.custom_alert_message( 2, "%s Enabled" % ("ACC" if self.adaptive else "CC"), 150) # Update the UI to show whether the current car state allows ACC. if CS.cstm_btns.get_button_status( ACCMode.BUTTON_NAME) in [ACCState.STANDBY, ACCState.NOT_READY]: if ((enabled or not self.adaptive) and CruiseState.is_enabled_or_standby(CS.pcm_acc_status) and CS.v_ego > self.MIN_CRUISE_SPEED_MS): CS.cstm_btns.set_button_status(ACCMode.BUTTON_NAME, ACCState.STANDBY) else: CS.cstm_btns.set_button_status(ACCMode.BUTTON_NAME, ACCState.NOT_READY) # Update prev state after all other actions. self.prev_cruise_buttons = CS.cruise_buttons self.prev_pcm_acc_status = CS.pcm_acc_status def _update_max_acc_speed(self, CS): # Adjust the max ACC speed based on user cruise stalk actions. half_press_kph, full_press_kph = self._get_cc_units_kph( CS.imperial_speed_units) speed_change_map = { CruiseButtons.RES_ACCEL: half_press_kph, CruiseButtons.RES_ACCEL_2ND: full_press_kph, CruiseButtons.DECEL_SET: -1 * half_press_kph, CruiseButtons.DECEL_2ND: -1 * full_press_kph } self.acc_speed_kph += speed_change_map.get(CS.cruise_buttons, 0) # Clip ACC speed between 0 and 170 KPH. self.acc_speed_kph = min(self.acc_speed_kph, 170) self.acc_speed_kph = max(self.acc_speed_kph, 0) # Decide which cruise control buttons to simluate to get the car to the desired speed. def update_acc(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_kph, set_speed_limit_active, speed_limit_offset): # Adaptive cruise control self.prev_speed_limit_kph = self.speed_limit_kph if set_speed_limit_active and speed_limit_kph > 0: self.speed_limit_kph = speed_limit_kph + speed_limit_offset if int(self.prev_speed_limit_kph) != int(self.speed_limit_kph): self.acc_speed_kph = self.speed_limit_kph self.fleet_speed.reset_averager() else: # reset internal speed limit, so double pull doesn't set higher speed than current (e.g. after leaving the highway) self.speed_limit_kph = 0. current_time_ms = _current_time_millis() if CruiseButtons.should_be_throttled(CS.cruise_buttons): self.human_cruise_action_time = current_time_ms button_to_press = None # If ACC is disabled, disengage traditional cruise control. if ((self.prev_enable_adaptive_cruise) and (not self.enable_adaptive_cruise) and (CS.pcm_acc_status == CruiseState.ENABLED)): button_to_press = CruiseButtons.CANCEL #if non addaptive and we just engaged ACC but pcm is not engaged, then engage if (not self.adaptive) and self.enable_adaptive_cruise and ( CS.pcm_acc_status != CruiseState.ENABLED): button_to_press = CruiseButtons.MAIN #if plain cc, not adaptive, then just return None or Cancel if (not self.adaptive) and self.enable_adaptive_cruise: self.acc_speed_kph = CS.v_cruise_actual #if not CS.imperial_speed_units else CS.v_cruise_actual * CV.MPH_TO_KPH return button_to_press #disengage if cruise is canceled if (not self.enable_adaptive_cruise) and (CS.pcm_acc_status >= 2) and ( CS.pcm_acc_status <= 4): button_to_press = CruiseButtons.CANCEL lead_1 = None #if enabled: lead = messaging.recv_one_or_none(self.radarState) if lead is not None: lead_1 = lead.radarState.leadOne if lead_1.dRel: self.lead_last_seen_time_ms = current_time_ms if self.enable_adaptive_cruise and enabled: # and (button_to_press == None): if CS.cstm_btns.get_button_label2( ACCMode.BUTTON_NAME) in ["OP", "AutoOP"]: button_to_press = self._calc_button(CS, pcm_speed) self.new_speed = pcm_speed * CV.MS_TO_KPH else: # Alternative speed decision logic that uses the lead car's distance # and speed more directly. # Bring in the lead car distance from the radarState feed button_to_press = self._calc_follow_button( CS, lead_1, speed_limit_kph, set_speed_limit_active, speed_limit_offset, frame) if button_to_press: self.automated_cruise_action_time = current_time_ms # If trying to slow below the min cruise speed, just cancel cruise. # This prevents a SCCM crash which is triggered by repeatedly pressing # stalk-down when already at min cruise speed. if (CruiseButtons.is_decel(button_to_press) and CS.v_cruise_actual - 1 < self.MIN_CRUISE_SPEED_MS * CV.MS_TO_KPH): button_to_press = CruiseButtons.CANCEL if button_to_press == CruiseButtons.CANCEL: self.fast_decel_time = current_time_ms # Debug logging (disable in production to reduce latency of commands) #print "***ACC command: %s***" % button_to_press return button_to_press # function to calculate the cruise button based on a safe follow distance def _calc_follow_button(self, CS, lead_car, speed_limit_kph, set_speed_limit_active, speed_limit_offset, frame): if lead_car is None: return None # Desired gap (in seconds) between cars. follow_time_s = CS.apFollowTimeInS # v_ego is in m/s, so safe_dist_m is in meters. safe_dist_m = CS.v_ego * follow_time_s current_time_ms = _current_time_millis() # Make sure we were able to populate lead_1. # dRel is in meters. lead_dist_m = lead_car.dRel lead_speed_kph = (lead_car.vRel + CS.v_ego) * CV.MS_TO_KPH # Relative velocity between the lead car and our set cruise speed. future_vrel_kph = lead_speed_kph - CS.v_cruise_actual # How much we can accelerate without exceeding the max allowed speed. max_acc_speed_kph = self.fleet_speed.adjust( CS, self.acc_speed_kph * CV.KPH_TO_MS, frame) * CV.MS_TO_KPH available_speed_kph = max_acc_speed_kph - CS.v_cruise_actual half_press_kph, full_press_kph = self._get_cc_units_kph( CS.imperial_speed_units) # button to issue button = None # debug msg msg = None # Automatically engage traditional cruise if ACC is active. if self._should_autoengage_cc( CS, lead_car=lead_car) and self._no_action_for(milliseconds=100): button = CruiseButtons.RES_ACCEL # If traditional cruise is engaged, then control it. elif CS.pcm_acc_status == CruiseState.ENABLED: # Disengage cruise control if a slow object is seen ahead. This triggers # full regen braking, which is stronger than the braking that happens if # you just reduce cruise speed. if self._fast_decel_required( CS, lead_car) and self._no_human_action_for(milliseconds=500): msg = "Off (Slow traffic)" button = CruiseButtons.CANCEL self.new_speed = 1 # if cruise is set to faster than the max speed, slow down elif CS.v_cruise_actual > max_acc_speed_kph and self._no_action_for( milliseconds=300): msg = "Slow to max" button = CruiseButtons.DECEL_SET self.new_speed = max_acc_speed_kph elif ( # if we have a populated lead_distance lead_dist_m > 0 and self._no_action_for(milliseconds=300) # and we're moving and CS.v_cruise_actual > full_press_kph): ### Slowing down ### # Reduce speed significantly if lead_dist < safe dist # and if the lead car isn't already pulling away. if lead_dist_m < safe_dist_m * .5 and future_vrel_kph < 2: msg = "-5 (Significantly too close)" button = CruiseButtons.DECEL_2ND self.new_speed = CS.v_ego * CV.MS_TO_KPH - full_press_kph # Don't rush up to lead car elif future_vrel_kph < -15: msg = "-5 (approaching too fast)" button = CruiseButtons.DECEL_2ND self.new_speed = CS.v_ego * CV.MS_TO_KPH - full_press_kph elif future_vrel_kph < -8: msg = "-1 (approaching too fast)" button = CruiseButtons.DECEL_SET self.new_speed = CS.v_ego * CV.MS_TO_KPH - half_press_kph elif lead_dist_m < safe_dist_m and future_vrel_kph <= 0: msg = "-1 (Too close)" button = CruiseButtons.DECEL_SET self.new_speed = CS.v_ego * CV.MS_TO_KPH - half_press_kph # Make slow adjustments if close to the safe distance. # only adjust every 1 secs elif (lead_dist_m < safe_dist_m * 1.3 and future_vrel_kph < -1 * half_press_kph and self._no_action_for(milliseconds=1000)): msg = "-1 (Near safe distance)" button = CruiseButtons.DECEL_SET self.new_speed = CS.v_ego * CV.MS_TO_KPH - half_press_kph ### Speed up ### elif (available_speed_kph > half_press_kph and lead_dist_m > safe_dist_m and self._no_human_action_for(milliseconds=1000)): lead_is_far = lead_dist_m > safe_dist_m * 1.75 closing = future_vrel_kph < -2 lead_is_pulling_away = future_vrel_kph > 4 if lead_is_far and not closing or lead_is_pulling_away: msg = "+1 (Beyond safe distance and speed)" button = CruiseButtons.RES_ACCEL self.new_speed = CS.v_ego * CV.MS_TO_KPH + half_press_kph # If lead_dist is reported as 0, no one is detected in front of you so you # can speed up. Only accel on straight-aways; vision radar often # loses lead car in a turn. elif (lead_dist_m == 0 and CS.angle_steers < 2.0 and half_press_kph < available_speed_kph and self._no_action_for(milliseconds=500) and self._no_human_action_for(milliseconds=1000) and current_time_ms > self.lead_last_seen_time_ms + 4000): msg = "+1 (road clear)" button = CruiseButtons.RES_ACCEL self.new_speed = CS.v_ego * CV.MS_TO_KPH + half_press_kph if (current_time_ms > self.last_update_time + 1000): ratio = 0 if safe_dist_m > 0: ratio = (lead_dist_m / safe_dist_m) * 100 print( "Ratio: {0:.1f}% lead: {1:.1f}m avail: {2:.1f}kph vRel: {3:.1f}kph Angle: {4:.1f}deg" .format(ratio, lead_dist_m, available_speed_kph, lead_car.vRel * CV.MS_TO_KPH, CS.angle_steers)) self.last_update_time = current_time_ms if msg is not None: print("ACC: " + msg) return button def _should_autoengage_cc(self, CS, lead_car=None): # Automatically (re)engage cruise control so long as # 1) The carstate allows cruise control # 2) There is no imminent threat of collision # 3) The user did not cancel ACC by pressing the brake cruise_ready = (self.enable_adaptive_cruise and CS.pcm_acc_status == CruiseState.STANDBY and CS.v_ego >= self.MIN_CRUISE_SPEED_MS and _current_time_millis() > self.fast_decel_time + 2000) slow_lead = lead_car and lead_car.dRel > 0 and lead_car.vRel < 0 or self._fast_decel_required( CS, lead_car) # pylint: disable=chained-comparison # "Autoresume" mode allows cruise to engage even after brake events, but # shouldn't trigger DURING braking. autoresume_ready = ( self.autoresume and CS.a_ego >= 0.1 and not self.CC.HSO.human_control and _current_time_millis() > self.last_brake_press_time + 1000) braked = self.user_has_braked or self.has_gone_below_min_speed return cruise_ready and not slow_lead and (autoresume_ready or not braked) def _fast_decel_required(self, CS, lead_car): """ Identifies situations which call for rapid deceleration. """ if not lead_car or not lead_car.dRel: return False collision_imminent = self._seconds_to_collision(CS, lead_car) < 4 lead_absolute_speed_ms = lead_car.vRel + CS.v_ego lead_too_slow = lead_absolute_speed_ms < self.MIN_CRUISE_SPEED_MS return collision_imminent or lead_too_slow def _seconds_to_collision(self, CS, lead_car): if not lead_car or not lead_car.dRel: return sys.maxsize elif lead_car.vRel >= 0: return sys.maxsize return abs(float(lead_car.dRel) / lead_car.vRel) def _get_cc_units_kph(self, is_imperial_units): # Cruise control buttons behave differently depending on whether the car # is configured for metric or imperial units. if is_imperial_units: # Imperial unit cars adjust cruise in units of 1 and 5 mph. half_press_kph = 1 * CV.MPH_TO_KPH full_press_kph = 5 * CV.MPH_TO_KPH else: # Metric cars adjust cruise in units of 1 and 5 kph. half_press_kph = 1 full_press_kph = 5 return half_press_kph, full_press_kph # Adjust speed based off OP's longitudinal model. As of OpenPilot 0.5.3, this # is inoperable because the planner crashes when given only visual radar # inputs. (Perhaps this can be used in the future with a radar install, or if # OpenPilot planner changes.) def _calc_button(self, CS, desired_speed_ms): button_to_press = None # Automatically engange traditional cruise if appropriate. if self._should_autoengage_cc(CS) and desired_speed_ms >= CS.v_ego: button_to_press = CruiseButtons.RES_ACCEL # If traditional cruise is engaged, then control it. elif (CS.pcm_acc_status == CruiseState.ENABLED # But don't make adjustments if a human has manually done so in # the last 3 seconds. Human intention should not be overridden. and self._no_human_action_for(milliseconds=3000) and self._no_automated_action_for(milliseconds=500)): # The difference between OP's target speed and the current cruise # control speed, in KPH. speed_offset_kph = (desired_speed_ms * CV.MS_TO_KPH - CS.v_cruise_actual) half_press_kph, full_press_kph = self._get_cc_units_kph( CS.imperial_speed_units) # Reduce cruise speed significantly if necessary. Multiply by a % to # make the car slightly more eager to slow down vs speed up. if desired_speed_ms < self.MIN_CRUISE_SPEED_MS: button_to_press = CruiseButtons.CANCEL if speed_offset_kph < -2 * full_press_kph and CS.v_cruise_actual > 0: button_to_press = CruiseButtons.CANCEL elif speed_offset_kph < -0.6 * full_press_kph and CS.v_cruise_actual > 0: # Send cruise stalk dn_2nd. button_to_press = CruiseButtons.DECEL_2ND # Reduce speed slightly if necessary. elif speed_offset_kph < -0.9 * half_press_kph and CS.v_cruise_actual > 0: # Send cruise stalk dn_1st. button_to_press = CruiseButtons.DECEL_SET # Increase cruise speed if possible. elif CS.v_ego > self.MIN_CRUISE_SPEED_MS: # How much we can accelerate without exceeding max allowed speed. available_speed_kph = self.acc_speed_kph - CS.v_cruise_actual if speed_offset_kph >= full_press_kph and full_press_kph < available_speed_kph: # Send cruise stalk up_2nd. button_to_press = CruiseButtons.RES_ACCEL_2ND elif speed_offset_kph >= half_press_kph and half_press_kph < available_speed_kph: # Send cruise stalk up_1st. button_to_press = CruiseButtons.RES_ACCEL return button_to_press def _no_human_action_for(self, milliseconds): return _current_time_millis( ) > self.human_cruise_action_time + milliseconds def _no_automated_action_for(self, milliseconds): return _current_time_millis( ) > self.automated_cruise_action_time + milliseconds def _no_action_for(self, milliseconds): return self._no_human_action_for( milliseconds) and self._no_automated_action_for(milliseconds)