def update(self, CS, CP, VM, PP, live20, live100, md, live_map_data): """Gets called when new live20 is available""" cur_time = live20.logMonoTime / 1e9 v_ego = CS.carState.vEgo long_control_state = live100.live100.longControlState v_cruise_kph = live100.live100.vCruise force_slow_decel = live100.live100.forceDecel v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS self.last_md_ts = md.logMonoTime self.radar_errors = list(live20.live20.radarErrors) self.lead_1 = live20.live20.leadOne self.lead_2 = live20.live20.leadTwo enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > v_ego and self.lead_1.aLeadK > 0.0 self.v_speedlimit = NO_CURVATURE_SPEED self.v_curvature = NO_CURVATURE_SPEED self.map_valid = live_map_data.liveMapData.mapValid # Speed limit and curvature set_speed_limit_active = self.params.get( "LimitSetSpeed") == "1" and self.params.get( "SpeedLimitOffset") is not None if set_speed_limit_active: if live_map_data.liveMapData.speedLimitValid: speed_limit = live_map_data.liveMapData.speedLimit offset = float(self.params.get("SpeedLimitOffset")) self.v_speedlimit = speed_limit + offset if live_map_data.liveMapData.curvatureValid: curvature = abs(live_map_data.liveMapData.curvature) a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph v_curvature = math.sqrt(a_y_max / max(1e-4, curvature)) self.v_curvature = min(NO_CURVATURE_SPEED, v_curvature) self.decel_for_turn = bool(self.v_curvature < min( [v_cruise_setpoint, self.v_speedlimit, v_ego + 1.])) v_cruise_setpoint = min( [v_cruise_setpoint, self.v_curvature, self.v_speedlimit]) # Calculate speed for normal cruise control if enabled: accel_limits = map(float, calc_cruise_accel_limits(v_ego, following)) jerk_limits = [ min(-0.1, accel_limits[0]), max(0.1, accel_limits[1]) ] # TODO: make a separate lookup for jerk tuning accel_limits = limit_accel_in_turns(v_ego, CS.carState.steeringAngle, accel_limits, self.CP) if force_slow_decel: # if required so, force a smooth deceleration accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL) accel_limits[0] = min(accel_limits[0], accel_limits[1]) # Change accel limits based on time remaining to turn if self.decel_for_turn: time_to_turn = max( 1.0, live_map_data.liveMapData.distToTurn / max(self.v_cruise, 1.)) required_decel = min(0, (self.v_curvature - self.v_cruise) / time_to_turn) accel_limits[0] = max(accel_limits[0], required_decel) self.v_cruise, self.a_cruise = speed_smoother( self.v_acc_start, self.a_acc_start, v_cruise_setpoint, accel_limits[1], accel_limits[0], jerk_limits[1], jerk_limits[0], _DT_MPC) # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) else: starting = long_control_state == LongCtrlState.starting a_ego = min(CS.carState.aEgo, 0.0) reset_speed = MIN_CAN_SPEED if starting else v_ego reset_accel = self.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.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc1.update(CS, self.lead_1, v_cruise_setpoint) self.mpc2.update(CS, self.lead_2, v_cruise_setpoint) self.choose_solution(v_cruise_setpoint, enabled) # determine fcw if self.mpc1.new_lead: self.fcw_checker.reset_lead(cur_time) blinkers = CS.carState.leftBlinker or CS.carState.rightBlinker self.fcw = self.fcw_checker.update( self.mpc1.mpc_solution, cur_time, v_ego, CS.carState.aEgo, self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK, self.lead_1.yRel, self.lead_1.vLat, self.lead_1.fcw, blinkers) and not CS.carState.brakePressed if self.fcw: cloudlog.info("FCW triggered %s", self.fcw_checker.counters) model_dead = cur_time - (md.logMonoTime / 1e9) > 0.5 # **** send the plan **** plan_send = messaging.new_message() plan_send.init('plan') # TODO: Move all these events to controlsd. This has nothing to do with planning events = [] if model_dead: events.append( create_event('modelCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if 'fault' in self.radar_errors: events.append( create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE])) plan_send.plan.events = events plan_send.plan.mdMonoTime = md.logMonoTime plan_send.plan.l20MonoTime = live20.logMonoTime # longitudal plan plan_send.plan.vCruise = self.v_cruise plan_send.plan.aCruise = self.a_cruise plan_send.plan.vStart = self.v_acc_start plan_send.plan.aStart = self.a_acc_start plan_send.plan.vTarget = self.v_acc plan_send.plan.aTarget = self.a_acc plan_send.plan.vTargetFuture = self.v_acc_future plan_send.plan.hasLead = self.mpc1.prev_lead_status plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource plan_send.plan.vCurvature = self.v_curvature plan_send.plan.decelForTurn = self.decel_for_turn plan_send.plan.mapValid = self.map_valid # Send out fcw fcw = self.fcw and (self.fcw_enabled or long_control_state != LongCtrlState.off) plan_send.plan.fcw = fcw self.plan.send(plan_send.to_bytes()) # Interpolate 0.05 seconds and save as starting point for next iteration dt = 0.05 # s a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc - self.a_acc_start) v_acc_sol = self.v_acc_start + dt * (a_acc_sol + self.a_acc_start) / 2.0 self.v_acc_start = v_acc_sol self.a_acc_start = a_acc_sol
def update(self, sm, CP): """Gets called when new radarState is available""" cur_time = sec_since_boot() v_ego = sm['carState'].vEgo long_control_state = sm['controlsState'].longControlState v_cruise_kph = sm['controlsState'].vCruise force_slow_decel = sm['controlsState'].forceDecel v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX) v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS lead_1 = sm['radarState'].leadOne lead_2 = sm['radarState'].leadTwo enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0 self.v_acc_start = self.v_acc_next self.a_acc_start = self.a_acc_next # Calculate speed for normal cruise control if enabled and not self.first_loop and not sm['carState'].gasPressed: accel_limits = [ float(x) for x in calc_cruise_accel_limits(v_ego, following) ] jerk_limits = [ min(-0.1, accel_limits[0]), max(0.1, accel_limits[1]) ] # TODO: make a separate lookup for jerk tuning accel_limits_turns = limit_accel_in_turns( v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP) if force_slow_decel: # if required so, force a smooth deceleration accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL) accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1]) self.v_cruise, self.a_cruise = speed_smoother( self.v_acc_start, self.a_acc_start, v_cruise_setpoint, accel_limits_turns[1], accel_limits_turns[0], jerk_limits[1], jerk_limits[0], LON_MPC_STEP) # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) else: starting = long_control_state == LongCtrlState.starting a_ego = min(sm['carState'].aEgo, 0.0) reset_speed = self.CP.minSpeedCan if starting else v_ego reset_accel = self.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.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc1.update(sm['carState'], lead_1) self.mpc2.update(sm['carState'], lead_2) self.choose_solution(v_cruise_setpoint, enabled) # determine fcw if self.mpc1.new_lead: self.fcw_checker.reset_lead(cur_time) blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker self.fcw = self.fcw_checker.update( self.mpc1.mpc_solution, cur_time, sm['controlsState'].active, v_ego, sm['carState'].aEgo, lead_1.dRel, lead_1.vLead, lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw, blinkers) and not sm['carState'].brakePressed if self.fcw: cloudlog.info("FCW triggered %s", self.fcw_checker.counters) # Interpolate 0.05 seconds and save as starting point for next iteration a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * ( self.a_acc - self.a_acc_start) v_acc_sol = self.v_acc_start + CP.radarTimeStep * ( a_acc_sol + self.a_acc_start) / 2.0 self.v_acc_next = v_acc_sol self.a_acc_next = a_acc_sol self.first_loop = False
def update(self, CS, LaC, LoC, v_cruise_kph, force_slow_decel): cur_time = sec_since_boot() v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS md = None l20 = None gps_planner_plan = None for socket, event in self.poller.poll(0): if socket is self.model: md = messaging.recv_one(socket) elif socket is self.live20: l20 = messaging.recv_one(socket) elif socket is self.gps_planner_plan: gps_planner_plan = messaging.recv_one(socket) if gps_planner_plan is not None: self.last_gps_planner_plan = gps_planner_plan if md is not None: self.last_md_ts = md.logMonoTime self.last_model = cur_time self.model_dead = False self.PP.update(CS.vEgo, md) if self.last_gps_planner_plan is not None: plan = self.last_gps_planner_plan.gpsPlannerPlan self.gps_planner_active = plan.valid if plan.valid: self.PP.d_poly = plan.poly self.PP.p_poly = plan.poly self.PP.c_poly = plan.poly self.PP.l_prob = 0.0 self.PP.r_prob = 0.0 self.PP.c_prob = 1.0 if l20 is not None: self.last_l20_ts = l20.logMonoTime self.last_l20 = cur_time self.radar_dead = False self.radar_errors = list(l20.live20.radarErrors) self.v_acc_start = self.v_acc_sol self.a_acc_start = self.a_acc_sol self.acc_start_time = cur_time self.lead_1 = l20.live20.leadOne self.lead_2 = l20.live20.leadTwo enabled = (LoC.long_control_state == LongCtrlState.pid) or (LoC.long_control_state == LongCtrlState.stopping) following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > CS.vEgo and self.lead_1.aLeadK > 0.0 # Calculate speed for normal cruise control if enabled: accel_limits = map(float, calc_cruise_accel_limits(CS.vEgo, following)) # TODO: make a separate lookup for jerk tuning jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] accel_limits = limit_accel_in_turns(CS.vEgo, CS.steeringAngle, accel_limits, self.CP) if force_slow_decel: # if required so, force a smooth deceleration accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL) accel_limits[0] = min(accel_limits[0], accel_limits[1]) self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start, v_cruise_setpoint, accel_limits[1], accel_limits[0], jerk_limits[1], jerk_limits[0], _DT_MPC) # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) else: starting = LoC.long_control_state == LongCtrlState.starting a_ego = min(CS.aEgo, 0.0) reset_speed = MIN_CAN_SPEED if starting else CS.vEgo reset_accel = self.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.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc1.update(CS, self.lead_1, v_cruise_setpoint) self.mpc2.update(CS, self.lead_2, v_cruise_setpoint) self.choose_solution(v_cruise_setpoint, enabled) # determine fcw if self.mpc1.new_lead: self.fcw_checker.reset_lead(cur_time) blinkers = CS.leftBlinker or CS.rightBlinker self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, CS.vEgo, CS.aEgo, self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK, self.lead_1.yRel, self.lead_1.vLat, self.lead_1.fcw, blinkers) \ and not CS.brakePressed if self.fcw: cloudlog.info("FCW triggered %s", self.fcw_checker.counters) if cur_time - self.last_model > 0.5: self.model_dead = True if cur_time - self.last_l20 > 0.5: self.radar_dead = True # **** send the plan **** plan_send = messaging.new_message() plan_send.init('plan') events = [] if self.model_dead: events.append(create_event('modelCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) # if self.radar_dead or 'commIssue' in self.radar_errors: # events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if 'fault' in self.radar_errors: events.append(create_event('radarFault', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if LaC.mpc_solution[0].cost > 10000. or LaC.mpc_nans: # TODO: find a better way to detect when MPC did not converge events.append(create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) # Interpolation of trajectory dt = min(cur_time - self.acc_start_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps 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 plan_send.plan.events = events plan_send.plan.mdMonoTime = self.last_md_ts plan_send.plan.l20MonoTime = self.last_l20_ts # lateral plan plan_send.plan.lateralValid = not self.model_dead plan_send.plan.dPoly = map(float, self.PP.d_poly) plan_send.plan.laneWidth = float(self.PP.lane_width) # longitudal plan plan_send.plan.longitudinalValid = not self.radar_dead plan_send.plan.vCruise = self.v_cruise plan_send.plan.aCruise = self.a_cruise plan_send.plan.vTarget = self.v_acc_sol plan_send.plan.aTarget = self.a_acc_sol plan_send.plan.vTargetFuture = self.v_acc_future plan_send.plan.hasLead = self.mpc1.prev_lead_status plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource plan_send.plan.gpsPlannerActive = self.gps_planner_active # Send out fcw fcw = self.fcw and (self.fcw_enabled or LoC.long_control_state != LongCtrlState.off) plan_send.plan.fcw = fcw self.plan.send(plan_send.to_bytes()) return plan_send
def update(self, CS, LoC, v_cruise_kph, user_distracted): cur_time = sec_since_boot() v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS md = messaging.recv_sock(self.model) if md is not None: self.last_md_ts = md.logMonoTime self.last_model = cur_time self.model_dead = False self.PP.update(CS.vEgo, md) l20 = messaging.recv_sock(self.live20) if md is None else None if l20 is not None: self.last_l20_ts = l20.logMonoTime self.last_l20 = cur_time self.radar_dead = False self.radar_errors = list(l20.live20.radarErrors) self.v_acc_start = self.v_acc_sol self.a_acc_start = self.a_acc_sol self.acc_start_time = cur_time self.lead_1 = l20.live20.leadOne self.lead_2 = l20.live20.leadTwo enabled = (LoC.long_control_state == LongCtrlState.pid) or (LoC.long_control_state == LongCtrlState.stopping) following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > CS.vEgo and self.lead_1.aLeadK > 0.0 # Calculate speed for normal cruise control if enabled: accel_limits = map( float, calc_cruise_accel_limits(CS.vEgo, following)) # TODO: make a separate lookup for jerk tuning jerk_limits = [ min(-0.1, accel_limits[0]), max(0.1, accel_limits[1]) ] accel_limits = limit_accel_in_turns(CS.vEgo, CS.steeringAngle, accel_limits, self.CP) if user_distracted: # if user is not responsive to awareness alerts, then start a smooth deceleration accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL) accel_limits[0] = min(accel_limits[0], accel_limits[1]) self.v_cruise, self.a_cruise = speed_smoother( self.v_acc_start, self.a_acc_start, v_cruise_setpoint, accel_limits[1], accel_limits[0], jerk_limits[1], jerk_limits[0], _DT_MPC) else: starting = LoC.long_control_state == LongCtrlState.starting self.v_cruise = CS.vEgo self.a_cruise = self.CP.startAccel if starting else CS.aEgo self.v_acc_start = CS.vEgo self.a_acc_start = self.CP.startAccel if starting else CS.aEgo self.v_acc = CS.vEgo self.a_acc = self.CP.startAccel if starting else CS.aEgo self.v_acc_sol = CS.vEgo self.a_acc_sol = self.CP.startAccel if starting else CS.aEgo self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc1.update(CS, self.lead_1, v_cruise_setpoint) self.mpc2.update(CS, self.lead_2, v_cruise_setpoint) self.choose_solution(v_cruise_setpoint) # determine fcw if self.mpc1.new_lead: self.fcw_checker.reset_lead(cur_time) blinkers = CS.leftBlinker or CS.rightBlinker self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, CS.vEgo, self.lead_1.vLead, self.lead_1.yRel, self.lead_1.vLat, self.lead_1.fcw, blinkers) \ and not CS.brakePressed if self.fcw: cloudlog.info("FCW triggered") if cur_time - self.last_model > 0.5: self.model_dead = True if cur_time - self.last_l20 > 0.5: self.radar_dead = True # **** send the plan **** plan_send = messaging.new_message() plan_send.init('plan') events = [] if self.model_dead: events.append( create_event('modelCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.radar_dead or 'commIssue' in self.radar_errors: events.append( create_event('radarCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if 'fault' in self.radar_errors: events.append( create_event('radarFault', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) # Interpolation of trajectory dt = min( cur_time - self.acc_start_time, _DT_MPC + _DT ) + _DT # no greater than dt mpc + dt, to prevent too high extraps 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 plan_send.plan.events = events plan_send.plan.mdMonoTime = self.last_md_ts plan_send.plan.l20MonoTime = self.last_l20_ts # lateral plan plan_send.plan.lateralValid = not self.model_dead plan_send.plan.dPoly = map(float, self.PP.d_poly) plan_send.plan.laneWidth = float(self.PP.lane_width) # longitudal plan plan_send.plan.longitudinalValid = not self.radar_dead plan_send.plan.vCruise = self.v_cruise plan_send.plan.aCruise = self.a_cruise plan_send.plan.vTarget = self.v_acc_sol plan_send.plan.aTarget = self.a_acc_sol plan_send.plan.vTargetFuture = self.v_acc_future plan_send.plan.hasLead = self.mpc1.prev_lead_status plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource # Send out fcw fcw = self.fcw and (self.fcw_enabled or LoC.long_control_state != LongCtrlState.off) plan_send.plan.fcw = fcw self.plan.send(plan_send.to_bytes()) return plan_send
def update_pdl(self, enabled, CS, frame, actuators, pcm_speed): cur_time = sec_since_boot() idx = self.pedal_idx self.pedal_idx = (self.pedal_idx + 1) % 16 if not CS.pedal_interceptor_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 Live20 feed l20 = None if enabled: for socket, _ in self.poller.poll(0): if socket is self.live20: l20 = messaging.recv_one(socket) break if l20 is not None: self.lead_1 = l20.live20.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 self.md_ts = l20.live20.mdMonoTime self.l100_ts = l20.live20.l100MonoTime brake_max, accel_max = calc_cruise_accel_limits(CS, self.lead_1) 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): self.v_pid = self.calc_follow_speed_ms(CS) # cruise speed can't be negative even is user is distracted self.v_pid = max(self.v_pid, 0.) enabled = self.enable_pedal_cruise and self.LoC.long_control_state in [ LongCtrlState.pid, LongCtrlState.stopping ] if self.enable_pedal_cruise: jerk_min, jerk_max = _jerk_limits(CS.v_ego, self.lead_1, self.pedal_speed_kph, self.lead_last_seen_time_ms) self.v_cruise, self.a_cruise = speed_smoother( self.v_acc_start, self.a_acc_start, self.v_pid, accel_max, brake_max, 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 self.v_acc_start = self.v_acc_sol self.a_acc_start = self.a_acc_sol self.acc_start_time = cur_time # Interpolation of trajectory dt = min( cur_time - self.acc_start_time, _DT_MPC + _DT ) + _DT # no greater than dt mpc + dt, to prevent too high extraps 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 # we will try to feed forward the pedal position.... we might want to feed the last output_gb.... # it's all about testing now. vTarget = clip(self.v_acc_sol, 0, self.v_pid) self.vTargetFuture = clip(self.v_acc_future, 0, self.v_pid) t_go, t_brake = self.LoC.update( self.enable_pedal_cruise, CS.v_ego, CS.brake_pressed != 0, CS.standstill, False, self.v_pid, vTarget, self.vTargetFuture, self.a_acc_sol, CS.CP, None) output_gb = t_go - t_brake #print "Output GB Follow:", output_gb else: self.LoC.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 ############################################################## # 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 ############################################################## # This is an experimental mode that is probably broken. # # Don't use it. # # Ratios are centered at 1. They can be multiplied together. # Factors are centered around 0. They can be multiplied by constants. # For example +9% is a 1.06 ratio or 0.09 factor. ############################################################## elif PCCModes.is_selected(ExperimentalMode(), CS.cstm_btns): output_gb = 0.0 if enabled and self.enable_pedal_cruise: MAX_DECEL_RATIO = 0 MAX_ACCEL_RATIO = 1.1 available_speed_kph = self.pedal_speed_kph - CS.v_ego * CV.MS_TO_KPH # Hold accel if radar gives intermittent readings at great distance. # Makes the car less skittish when first making radar contact. if (_is_present(self.lead_1) and self.continuous_lead_sightings < 8 and _sec_til_collision(self.lead_1) > 3 and self.lead_1.dRel > 60): output_gb = self.last_output_gb # Hold speed in turns if no car is seen elif CS.angle_steers >= 5.0 and not _is_present(self.lead_1): pass # Try to stay 2 seconds behind lead, matching their speed. elif _is_present(self.lead_1): weighted_d_ratio = _weighted_distance_ratio( self.lead_1, CS.v_ego, MAX_DECEL_RATIO, MAX_ACCEL_RATIO) weighted_v_ratio = _weighted_velocity_ratio( self.lead_1, CS.v_ego, MAX_DECEL_RATIO, MAX_ACCEL_RATIO) # Don't bother decelerating if the lead is already pulling away if weighted_d_ratio < 1 and weighted_v_ratio > 1.01: gas_brake_ratio = max(1, self.last_output_gb + 1) else: gas_brake_ratio = weighted_d_ratio * weighted_v_ratio # rescale around 0 rather than 1. output_gb = gas_brake_ratio - 1 # If no lead has been seen recently, accelerate to max speed. else: # An acceleration factor that drops off as we aproach max speed. max_speed_factor = min(available_speed_kph, 3) / 3 # An acceleration factor that increases as time passes without seeing # a lead car. time_factor = (_current_time_millis() - self.lead_last_seen_time_ms) / 3000 time_factor = clip(time_factor, 0, 1) output_gb = 0.14 * max_speed_factor * time_factor # If going above the max configured PCC speed, slow. This should always # be in force so it is not part of the if/else block above. if available_speed_kph < 0: # linearly brake harder, hitting -1 at 10kph over speed_limited_gb = max(available_speed_kph, -10) / 10.0 # This is a minimum braking amount. The logic above may ask for more. output_gb = min(output_gb, speed_limited_gb) ###################################################################################### # 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)): 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) self.last_output_gb = output_gb # accel and brake apply_accel = clip(output_gb, 0., accel_max) MPC_BRAKE_MULTIPLIER = 6. apply_brake = -clip(output_gb * MPC_BRAKE_MULTIPLIER, -brake_max, 0.) # if speed is over 5mpg, 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, 0, MAX_PEDAL_VALUE - tesla_brake) tesla_pedal = tesla_brake + tesla_accel tesla_pedal, self.accel_steady = accel_hysteresis( tesla_pedal, self.accel_steady, 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 self.last_md_ts = self.md_ts self.last_l100_ts = self.l100_ts return self.prev_tesla_pedal, enable_pedal, idx
def update(self, sm, pm, CP, VM, PP): """Gets called when new radarState is available""" cur_time = sec_since_boot() v_ego = sm['carState'].vEgo long_control_state = sm['controlsState'].longControlState v_cruise_kph = sm['controlsState'].vCruise force_slow_decel = sm['controlsState'].forceDecel v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX) v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS lead_1 = sm['radarState'].leadOne lead_2 = sm['radarState'].leadTwo enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0 # dp self.dp_profile = sm['dragonConf'].dpAccelProfile self.dp_slow_on_curve = sm['dragonConf'].dpSlowOnCurve # dp - slow on curve from 0.7.6.1 if self.dp_slow_on_curve and len(sm['model'].path.poly): path = list(sm['model'].path.poly) # Curvature of polynomial https://en.wikipedia.org/wiki/Curvature#Curvature_of_the_graph_of_a_function # y = a x^3 + b x^2 + c x + d, y' = 3 a x^2 + 2 b x + c, y'' = 6 a x + 2 b # k = y'' / (1 + y'^2)^1.5 # TODO: compute max speed without using a list of points and without numpy y_p = 3 * path[0] * self.path_x**2 + 2 * path[ 1] * self.path_x + path[2] y_pp = 6 * path[0] * self.path_x + 2 * path[1] curv = y_pp / (1. + y_p**2)**1.5 a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 1e-4, None)) model_speed = np.min(v_curvature) model_speed = max(20.0 * CV.MPH_TO_MS, model_speed) # Don't slow down below 20mph else: model_speed = MAX_SPEED # Calculate speed for normal cruise control pedal_pressed = sm['carState'].gasPressed or sm['carState'].brakePressed if enabled and not self.first_loop and not pedal_pressed: if self.dp_profile == DP_OFF: accel_limits = [ float(x) for x in calc_cruise_accel_limits(v_ego, following) ] else: accel_limits = [ float(x) for x in dp_calc_cruise_accel_limits( v_ego, following, self.dp_profile) ] jerk_limits = [ min(-0.1, accel_limits[0]), max(0.1, accel_limits[1]) ] # TODO: make a separate lookup for jerk tuning accel_limits_turns = limit_accel_in_turns( v_ego, sm['carState'].steeringAngle, accel_limits, self.CP) if force_slow_decel: # if required so, force a smooth deceleration accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL) accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1]) self.v_cruise, self.a_cruise = speed_smoother( self.v_acc_start, self.a_acc_start, v_cruise_setpoint, accel_limits_turns[1], accel_limits_turns[0], jerk_limits[1], jerk_limits[0], LON_MPC_STEP) # dp - slow on curve from 0.7.6.1 if self.dp_slow_on_curve: self.v_model, self.a_model = speed_smoother( self.v_acc_start, self.a_acc_start, model_speed, 2 * accel_limits[1], accel_limits[0], 2 * jerk_limits[1], jerk_limits[0], LON_MPC_STEP) # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) else: starting = long_control_state == LongCtrlState.starting a_ego = min(sm['carState'].aEgo, 0.0) reset_speed = MIN_CAN_SPEED if starting else v_ego reset_accel = self.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.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc1.update(pm, sm['carState'], lead_1) self.mpc2.update(pm, sm['carState'], lead_2) self.choose_solution(v_cruise_setpoint, enabled) # determine fcw if self.mpc1.new_lead: self.fcw_checker.reset_lead(cur_time) blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker fcw = self.fcw_checker.update( self.mpc1.mpc_solution, cur_time, sm['controlsState'].active, v_ego, sm['carState'].aEgo, lead_1.dRel, lead_1.vLead, lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw, blinkers) and not sm['carState'].brakePressed if fcw: cloudlog.info("FCW triggered %s", self.fcw_checker.counters) radar_dead = not sm.alive['radarState'] radar_errors = list(sm['radarState'].radarErrors) radar_fault = car.RadarData.Error.fault in radar_errors radar_can_error = car.RadarData.Error.canError in radar_errors # **** send the plan **** plan_send = messaging.new_message('plan') plan_send.valid = sm.all_alive_and_valid( service_list=['carState', 'controlsState', 'radarState']) plan_send.plan.mdMonoTime = sm.logMonoTime['model'] plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState'] # longitudal plan plan_send.plan.vCruise = float(self.v_cruise) plan_send.plan.aCruise = float(self.a_cruise) plan_send.plan.vStart = float(self.v_acc_start) plan_send.plan.aStart = float(self.a_acc_start) plan_send.plan.vTarget = float(self.v_acc) plan_send.plan.aTarget = float(self.a_acc) plan_send.plan.vTargetFuture = float(self.v_acc_future) plan_send.plan.hasLead = self.mpc1.prev_lead_status plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource radar_valid = not (radar_dead or radar_fault) plan_send.plan.radarValid = bool(radar_valid) plan_send.plan.radarCanError = bool(radar_can_error) plan_send.plan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState'] # Send out fcw plan_send.plan.fcw = fcw pm.send('plan', plan_send) # Interpolate 0.05 seconds and save as starting point for next iteration a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * ( self.a_acc - self.a_acc_start) v_acc_sol = self.v_acc_start + CP.radarTimeStep * ( a_acc_sol + self.a_acc_start) / 2.0 self.v_acc_start = v_acc_sol self.a_acc_start = a_acc_sol self.first_loop = False
def update(self, sm, pm, CP, VM, PP): """Gets called when new radarState is available""" cur_time = sec_since_boot() v_ego = sm['carState'].vEgo long_control_state = sm['controlsState'].longControlState v_cruise_kph = sm['controlsState'].vCruise force_slow_decel = sm['controlsState'].forceDecel v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX) v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS lead_1 = sm['radarState'].leadOne lead_2 = sm['radarState'].leadTwo enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0 # Calculate speed for normal cruise control if enabled and not self.first_loop and not sm['carState'].gasPressed: accel_limits = [ float(x) for x in calc_cruise_accel_limits(v_ego, following) ] jerk_limits = [ min(-0.1, accel_limits[0]), max(0.1, accel_limits[1]) ] # TODO: make a separate lookup for jerk tuning accel_limits_turns = limit_accel_in_turns( v_ego, sm['carState'].steeringAngle, accel_limits, self.CP) if force_slow_decel: # if required so, force a smooth deceleration accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL) accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1]) self.v_cruise, self.a_cruise = speed_smoother( self.v_acc_start, self.a_acc_start, v_cruise_setpoint, accel_limits_turns[1], accel_limits_turns[0], jerk_limits[1], jerk_limits[0], LON_MPC_STEP) # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) else: starting = long_control_state == LongCtrlState.starting a_ego = min(sm['carState'].aEgo, 0.0) reset_speed = self.CP.minSpeedCan if starting else v_ego reset_accel = self.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.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc1.update(pm, sm['carState'], lead_1) self.mpc2.update(pm, sm['carState'], lead_2) self.choose_solution(v_cruise_setpoint, enabled) # determine fcw if self.mpc1.new_lead: self.fcw_checker.reset_lead(cur_time) blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker fcw = self.fcw_checker.update( self.mpc1.mpc_solution, cur_time, sm['controlsState'].active, v_ego, sm['carState'].aEgo, lead_1.dRel, lead_1.vLead, lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw, blinkers) and not sm['carState'].brakePressed if fcw: cloudlog.info("FCW triggered %s", self.fcw_checker.counters) radar_dead = not sm.alive['radarState'] radar_errors = list(sm['radarState'].radarErrors) radar_fault = car.RadarData.Error.fault in radar_errors radar_can_error = car.RadarData.Error.canError in radar_errors # **** send the plan **** plan_send = messaging.new_message('plan') plan_send.valid = sm.all_alive_and_valid( service_list=['carState', 'controlsState', 'radarState']) plan_send.plan.mdMonoTime = sm.logMonoTime['model'] plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState'] # longitudal plan plan_send.plan.vCruise = float(self.v_cruise) plan_send.plan.aCruise = float(self.a_cruise) plan_send.plan.vStart = float(self.v_acc_start) plan_send.plan.aStart = float(self.a_acc_start) plan_send.plan.vTarget = float(self.v_acc) plan_send.plan.aTarget = float(self.a_acc) plan_send.plan.vTargetFuture = float(self.v_acc_future) plan_send.plan.hasLead = self.mpc1.prev_lead_status plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource radar_valid = not (radar_dead or radar_fault) plan_send.plan.radarValid = bool(radar_valid) plan_send.plan.radarCanError = bool(radar_can_error) plan_send.plan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState'] # Send out fcw plan_send.plan.fcw = fcw pm.send('plan', plan_send) # Interpolate 0.05 seconds and save as starting point for next iteration a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * ( self.a_acc - self.a_acc_start) v_acc_sol = self.v_acc_start + CP.radarTimeStep * ( a_acc_sol + self.a_acc_start) / 2.0 self.v_acc_start = v_acc_sol self.a_acc_start = a_acc_sol self.first_loop = False
def update(self, sm, CP, VM, PP, live_map_data): """Gets called when new radarState is available""" cur_time = sec_since_boot() v_ego = sm['carState'].vEgo long_control_state = sm['controlsState'].longControlState v_cruise_kph = sm['controlsState'].vCruise force_slow_decel = sm['controlsState'].forceDecel v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS lead_1 = sm['radarState'].leadOne lead_2 = sm['radarState'].leadTwo enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0 v_speedlimit = NO_CURVATURE_SPEED v_curvature = NO_CURVATURE_SPEED #map_age = cur_time - rcv_times['liveMapData'] map_valid = False # live_map_data.liveMapData.mapValid and map_age < 10.0 # Speed limit and curvature set_speed_limit_active = self.params.get( "LimitSetSpeed") == "1" and self.params.get( "SpeedLimitOffset") is not None if set_speed_limit_active and map_valid: if live_map_data.liveMapData.speedLimitValid: speed_limit = live_map_data.liveMapData.speedLimit offset = float(self.params.get("SpeedLimitOffset")) v_speedlimit = speed_limit + offset if live_map_data.liveMapData.curvatureValid: curvature = abs(live_map_data.liveMapData.curvature) a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph v_curvature = math.sqrt(a_y_max / max(1e-4, curvature)) v_curvature = min(NO_CURVATURE_SPEED, v_curvature) decel_for_turn = bool( v_curvature < min([v_cruise_setpoint, v_speedlimit, v_ego + 1.])) v_cruise_setpoint = min([v_cruise_setpoint, v_curvature, v_speedlimit]) # Calculate speed for normal cruise control if enabled: accel_limits = [ float(x) for x in calc_cruise_accel_limits(v_ego, following) ] jerk_limits = [ min(-0.1, accel_limits[0]), max(0.1, accel_limits[1]) ] # TODO: make a separate lookup for jerk tuning accel_limits = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP) if force_slow_decel: # if required so, force a smooth deceleration accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL) accel_limits[0] = min(accel_limits[0], accel_limits[1]) # Change accel limits based on time remaining to turn if decel_for_turn: time_to_turn = max( 1.0, live_map_data.liveMapData.distToTurn / max(self.v_cruise, 1.)) required_decel = min(0, (v_curvature - self.v_cruise) / time_to_turn) accel_limits[0] = max(accel_limits[0], required_decel) self.v_cruise, self.a_cruise = speed_smoother( self.v_acc_start, self.a_acc_start, v_cruise_setpoint, accel_limits[1], accel_limits[0], jerk_limits[1], jerk_limits[0], LON_MPC_STEP) # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) else: starting = long_control_state == LongCtrlState.starting a_ego = min(sm['carState'].aEgo, 0.0) reset_speed = MIN_CAN_SPEED if starting else v_ego reset_accel = self.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.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc1.update(sm['carState'], lead_1, v_cruise_setpoint) self.mpc2.update(sm['carState'], lead_2, v_cruise_setpoint) self.choose_solution(v_cruise_setpoint, enabled) # determine fcw if self.mpc1.new_lead: self.fcw_checker.reset_lead(cur_time) blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker fcw = self.fcw_checker.update( self.mpc1.mpc_solution, cur_time, v_ego, sm['carState'].aEgo, lead_1.dRel, lead_1.vLead, lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw, blinkers) and not sm['carState'].brakePressed if fcw: cloudlog.info("FCW triggered %s", self.fcw_checker.counters) radar_dead = not sm.alive['radarState'] radar_errors = list(sm['radarState'].radarErrors) radar_fault = car.RadarData.Error.fault in radar_errors radar_can_error = car.RadarData.Error.canError in radar_errors # **** send the plan **** plan_send = messaging.new_message() plan_send.init('plan') plan_send.valid = sm.all_alive_and_valid( service_list=['carState', 'controlsState', 'radarState']) plan_send.plan.mdMonoTime = sm.logMonoTime['model'] plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState'] # longitudal plan plan_send.plan.vCruise = self.v_cruise plan_send.plan.aCruise = self.a_cruise plan_send.plan.vStart = self.v_acc_start plan_send.plan.aStart = self.a_acc_start plan_send.plan.vTarget = self.v_acc plan_send.plan.aTarget = self.a_acc plan_send.plan.vTargetFuture = self.v_acc_future plan_send.plan.hasLead = self.mpc1.prev_lead_status plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource plan_send.plan.vCurvature = v_curvature plan_send.plan.decelForTurn = decel_for_turn plan_send.plan.mapValid = map_valid radar_valid = not (radar_dead or radar_fault) plan_send.plan.radarValid = bool(radar_valid) plan_send.plan.radarCanError = bool(radar_can_error) plan_send.plan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState'] # Send out fcw fcw = fcw and (self.fcw_enabled or long_control_state != LongCtrlState.off) plan_send.plan.fcw = fcw self.plan.send(plan_send.to_bytes()) # Interpolate 0.05 seconds and save as starting point for next iteration a_acc_sol = self.a_acc_start + (DT_PLAN / LON_MPC_STEP) * ( self.a_acc - self.a_acc_start) v_acc_sol = self.v_acc_start + DT_PLAN * (a_acc_sol + self.a_acc_start) / 2.0 self.v_acc_start = v_acc_sol self.a_acc_start = a_acc_sol
def update(self, sm, pm, CP, VM, PP): self.arne182 = None for socket, _ in self.poller.poll(0): if socket is self.arne182Status: self.arne182 = arne182.Arne182Status.from_bytes(socket.recv()) if self.arne182 is None: gasbuttonstatus = 0 else: gasbuttonstatus = self.arne182.gasbuttonstatus """Gets called when new radarState is available""" cur_time = sec_since_boot() v_ego = sm['carState'].vEgo if gasbuttonstatus == 1: speed_ahead_distance = 150 elif gasbuttonstatus == 2: speed_ahead_distance = 350 else: speed_ahead_distance = 250 long_control_state = sm['controlsState'].longControlState v_cruise_kph = sm['controlsState'].vCruise force_slow_decel = sm['controlsState'].forceDecel v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS lead_1 = sm['radarState'].leadOne lead_2 = sm['radarState'].leadTwo enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0 v_speedlimit = NO_CURVATURE_SPEED v_curvature_map = NO_CURVATURE_SPEED v_speedlimit_ahead = NO_CURVATURE_SPEED if len(sm['model'].path.poly): path = list(sm['model'].path.poly) # Curvature of polynomial https://en.wikipedia.org/wiki/Curvature#Curvature_of_the_graph_of_a_function # y = a x^3 + b x^2 + c x + d, y' = 3 a x^2 + 2 b x + c, y'' = 6 a x + 2 b # k = y'' / (1 + y'^2)^1.5 # TODO: compute max speed without using a list of points and without numpy y_p = 3 * path[0] * self.path_x**2 + 2 * path[ 1] * self.path_x + path[2] y_pp = 6 * path[0] * self.path_x + 2 * path[1] curv = y_pp / (1. + y_p**2)**1.5 a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 1e-4, None)) model_speed = np.min(v_curvature) model_speed = max(20.0 * CV.MPH_TO_MS, model_speed) # Don't slow down below 20mph else: model_speed = MAX_SPEED now = datetime.now() try: if sm['liveMapData'].speedLimitValid and osm and ( sm['liveMapData'].lastGps.timestamp - time.mktime(now.timetuple()) * 1000) < 10000: speed_limit = sm['liveMapData'].speedLimit v_speedlimit = speed_limit + offset else: speed_limit = None if sm['liveMapData'].speedLimitAheadValid and sm[ 'liveMapData'].speedLimitAheadDistance < speed_ahead_distance and ( sm['liveMapData'].lastGps.timestamp - time.mktime(now.timetuple()) * 1000) < 10000: distanceatlowlimit = 50 if sm['liveMapData'].speedLimitAhead < 21 / 3.6: distanceatlowlimit = speed_ahead_distance = ( v_ego - sm['liveMapData'].speedLimitAhead) * 3.6 * 2 if distanceatlowlimit < 50: distanceatlowlimit = 0 distanceatlowlimit = min(distanceatlowlimit, 100) speed_ahead_distance = ( v_ego - sm['liveMapData'].speedLimitAhead) * 3.6 * 5 speed_ahead_distance = min(speed_ahead_distance, 300) speed_ahead_distance = max(speed_ahead_distance, 50) if speed_limit is not None and sm[ 'liveMapData'].speedLimitAheadDistance > distanceatlowlimit and v_ego + 3 < sm[ 'liveMapData'].speedLimitAhead + ( speed_limit - sm['liveMapData'].speedLimitAhead ) * sm[ 'liveMapData'].speedLimitAheadDistance / speed_ahead_distance: speed_limit_ahead = sm['liveMapData'].speedLimitAhead + ( speed_limit - sm['liveMapData'].speedLimitAhead) * ( sm['liveMapData'].speedLimitAheadDistance - distanceatlowlimit) / (speed_ahead_distance - distanceatlowlimit) else: speed_limit_ahead = sm['liveMapData'].speedLimitAhead v_speedlimit_ahead = speed_limit_ahead + offset if sm['liveMapData'].curvatureValid and osm and ( sm['liveMapData'].lastGps.timestamp - time.mktime(now.timetuple()) * 1000) < 10000: curvature = abs(sm['liveMapData'].curvature) radius = 1 / max(1e-4, curvature) if radius > 500: c = 0.7 # 0.7 at 1000m = 95 kph elif radius > 250: c = 2.7 - 1 / 250 * radius # 1.7 at 264m 76 kph else: c = 3.0 - 13 / 2500 * radius # 3.0 at 15m 24 kph v_curvature_map = math.sqrt(c * radius) v_curvature_map = min(NO_CURVATURE_SPEED, v_curvature_map) except KeyError: pass decel_for_turn = bool(v_curvature_map < min( [v_cruise_setpoint, v_speedlimit, v_ego + 1.])) v_cruise_setpoint = min([ v_cruise_setpoint, v_curvature_map, v_speedlimit, v_speedlimit_ahead ]) # Calculate speed for normal cruise control if enabled: accel_limits = [ float(x) for x in calc_cruise_accel_limits( v_ego, following, gasbuttonstatus) ] jerk_limits = [ min(-0.1, accel_limits[0]), max(0.1, accel_limits[1]) ] # TODO: make a separate lookup for jerk tuning accel_limits_turns = limit_accel_in_turns( v_ego, sm['carState'].steeringAngle, accel_limits, self.CP) if force_slow_decel: # if required so, force a smooth deceleration accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL) accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1]) if decel_for_turn and sm[ 'liveMapData'].distToTurn < speed_ahead_distance: time_to_turn = max( 1.0, sm['liveMapData'].distToTurn / max( (v_ego + v_curvature_map) / 2, 1.)) required_decel = min(0, (v_curvature_map - v_ego) / time_to_turn) accel_limits[0] = max(accel_limits[0], required_decel) if v_speedlimit_ahead < v_speedlimit and self.longitudinalPlanSource == 'cruise' and v_ego > v_speedlimit_ahead: required_decel = min( 0, (v_speedlimit_ahead * v_speedlimit_ahead - v_ego * v_ego) / (sm['liveMapData'].speedLimitAheadDistance * 2)) required_decel = max(required_decel, -3.0) accel_limits[0] = required_decel accel_limits[1] = required_decel self.a_acc_start = required_decel self.v_cruise, self.a_cruise = speed_smoother( self.v_acc_start, self.a_acc_start, v_cruise_setpoint, accel_limits_turns[1], accel_limits_turns[0], jerk_limits[1], jerk_limits[0], LON_MPC_STEP) self.v_model, self.a_model = speed_smoother( self.v_acc_start, self.a_acc_start, model_speed, 2 * accel_limits[1], accel_limits[0], 2 * jerk_limits[1], jerk_limits[0], LON_MPC_STEP) # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) else: starting = long_control_state == LongCtrlState.starting a_ego = min(sm['carState'].aEgo, 0.0) reset_speed = MIN_CAN_SPEED if starting else v_ego reset_accel = self.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.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc1.update(pm, sm['carState'], lead_1, v_cruise_setpoint) self.mpc2.update(pm, sm['carState'], lead_2, v_cruise_setpoint) self.choose_solution(v_cruise_setpoint, enabled) # determine fcw if self.mpc1.new_lead: self.fcw_checker.reset_lead(cur_time) blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker fcw = self.fcw_checker.update( self.mpc1.mpc_solution, cur_time, sm['controlsState'].active, v_ego, sm['carState'].aEgo, lead_1.dRel, lead_1.vLead, lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw, blinkers) and not sm['carState'].brakePressed if fcw: cloudlog.info("FCW triggered %s", self.fcw_checker.counters) radar_dead = not sm.alive['radarState'] radar_errors = list(sm['radarState'].radarErrors) radar_fault = car.RadarData.Error.fault in radar_errors radar_can_error = car.RadarData.Error.canError in radar_errors # **** send the plan **** plan_send = messaging.new_message() plan_send.init('plan') plan_send.valid = sm.all_alive_and_valid( service_list=['carState', 'controlsState', 'radarState']) plan_send.plan.mdMonoTime = sm.logMonoTime['model'] plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState'] # longitudal plan plan_send.plan.vCruise = float(self.v_cruise) plan_send.plan.aCruise = float(self.a_cruise) plan_send.plan.vStart = float(self.v_acc_start) plan_send.plan.aStart = float(self.a_acc_start) plan_send.plan.vTarget = float(self.v_acc) plan_send.plan.aTarget = float(self.a_acc) plan_send.plan.vTargetFuture = float(self.v_acc_future) plan_send.plan.hasLead = self.mpc1.prev_lead_status plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource plan_send.plan.vCurvature = float(v_curvature_map) plan_send.plan.decelForTurn = bool( decel_for_turn or v_speedlimit_ahead < min([v_speedlimit, v_ego + 1.])) plan_send.plan.mapValid = True radar_valid = not (radar_dead or radar_fault) plan_send.plan.radarValid = bool(radar_valid) plan_send.plan.radarCanError = bool(radar_can_error) plan_send.plan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState'] # Send out fcw plan_send.plan.fcw = fcw pm.send('plan', plan_send) # Interpolate 0.05 seconds and save as starting point for next iteration a_acc_sol = self.a_acc_start + (DT_PLAN / LON_MPC_STEP) * ( self.a_acc - self.a_acc_start) v_acc_sol = self.v_acc_start + DT_PLAN * (a_acc_sol + self.a_acc_start) / 2.0 self.v_acc_start = v_acc_sol self.a_acc_start = a_acc_sol
def update_pdl(self, enabled, CS, frame, actuators, pcm_speed): cur_time = sec_since_boot() idx = self.pedal_idx self.pedal_idx = (self.pedal_idx + 1) % 16 if not CS.pedal_interceptor_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 Live20 feed l20 = None if enabled: for socket, _ in self.poller.poll(0): if socket is self.live20: l20 = messaging.recv_one(socket) break if l20 is not None: self.lead_1 = l20.live20.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 self.md_ts = l20.live20.mdMonoTime self.l100_ts = l20.live20.l100MonoTime brake_max, accel_max = calc_cruise_accel_limits(CS, self.lead_1) 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): self.v_pid = self.calc_follow_speed_ms(CS) # cruise speed can't be negative even is user is distracted self.v_pid = max(self.v_pid, 0.) enabled = self.enable_pedal_cruise and self.LoC.long_control_state in [ LongCtrlState.pid, LongCtrlState.stopping ] if self.enable_pedal_cruise: jerk_min, jerk_max = _jerk_limits(CS.v_ego, self.lead_1, self.pedal_speed_kph, self.lead_last_seen_time_ms) self.v_cruise, self.a_cruise = speed_smoother( self.v_acc_start, self.a_acc_start, self.v_pid, accel_max, brake_max, 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 self.v_acc_start = self.v_acc_sol self.a_acc_start = self.a_acc_sol self.acc_start_time = cur_time # Interpolation of trajectory dt = min( cur_time - self.acc_start_time, _DT_MPC + _DT ) + _DT # no greater than dt mpc + dt, to prevent too high extraps 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 # we will try to feed forward the pedal position.... we might want to feed the last output_gb.... # it's all about testing now. vTarget = clip(self.v_acc_sol, 0, self.v_pid) self.vTargetFuture = clip(self.v_acc_future, 0, self.v_pid) t_go, t_brake = self.LoC.update( self.enable_pedal_cruise, CS.v_ego, CS.brake_pressed != 0, CS.standstill, False, self.v_pid, vTarget, self.vTargetFuture, self.a_acc_sol, CS.CP, None) output_gb = t_go - t_brake #print "Output GB Follow:", output_gb else: self.LoC.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 ############################################################## # 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 ###################################################################################### # 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)): 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) self.last_output_gb = output_gb # accel and brake apply_accel = clip(output_gb, 0., accel_max) MPC_BRAKE_MULTIPLIER = 6. apply_brake = -clip(output_gb * MPC_BRAKE_MULTIPLIER, -brake_max, 0.) # if speed is over 5mpg, 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, 0, MAX_PEDAL_VALUE - tesla_brake) tesla_pedal = tesla_brake + tesla_accel tesla_pedal, self.accel_steady = accel_hysteresis( tesla_pedal, self.accel_steady, 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 self.last_md_ts = self.md_ts self.last_l100_ts = self.l100_ts return self.prev_tesla_pedal, enable_pedal, idx
def update(self, sm, CP): """Gets called when new radarState is available""" cur_time = sec_since_boot() v_ego = sm['carState'].vEgo self.vego = v_ego long_control_state = sm['controlsState'].longControlState v_cruise_kph = sm['controlsState'].vCruise force_slow_decel = sm['controlsState'].forceDecel v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX) v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS lead_1 = sm['radarState'].leadOne lead_2 = sm['radarState'].leadTwo enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0 self.v_acc_start = self.v_acc_next self.a_acc_start = self.a_acc_next self.target_speed_map_counter += 1 if self.target_speed_map_counter >= ( 50 + self.target_speed_map_counter1 ) and self.target_speed_map_counter_check == False: self.target_speed_map_counter_check = True os.system( "logcat -d -s opkrspdlimit,opkrspd2limit | grep opkrspd | tail -n 1 | awk \'{print $7}\' > /data/params/d/LimitSetSpeedCamera &" ) os.system( "logcat -d -s opkrspddist | grep opkrspd | tail -n 1 | awk \'{print $7}\' > /data/params/d/LimitSetSpeedCameraDist &" ) self.target_speed_map_counter3 += 1 if self.target_speed_map_counter3 > 2: self.target_speed_map_counter3 = 0 os.system("logcat -c &") elif self.target_speed_map_counter >= (75 + self.target_speed_map_counter1): self.target_speed_map_counter1 = 0 self.target_speed_map_counter = 0 self.target_speed_map_counter_check = False mapspeed = self.params.get("LimitSetSpeedCamera", encoding="utf8") mapspeeddist = self.params.get("LimitSetSpeedCameraDist", encoding="utf8") if mapspeed is not None and mapspeeddist is not None: mapspeed = int(float(mapspeed.rstrip('\n'))) mapspeeddist = int(float(mapspeeddist.rstrip('\n'))) if mapspeed > 29: self.target_speed_map = mapspeed self.target_speed_map_dist = mapspeeddist if self.target_speed_map_dist > 1001: self.target_speed_map_block = True self.target_speed_map_counter1 = 80 os.system("echo -n 1 > /data/params/d/OpkrSafetyCamera &") os.system("logcat -c &") else: self.target_speed_map = 0 self.target_speed_map_dist = 0 self.target_speed_map_block = False elif mapspeed is None and mapspeeddist is None and self.target_speed_map_counter2 < 2: self.target_speed_map_counter2 += 1 self.target_speed_map_counter = 51 self.target_speed_map = 0 self.target_speed_map_dist = 0 self.target_speed_map_counter_check = True self.target_speed_map_block = False self.target_speed_map_sign = False else: self.target_speed_map_counter = 49 self.target_speed_map_counter2 = 0 self.target_speed_map = 0 self.target_speed_map_dist = 0 self.target_speed_map_counter_check = False self.target_speed_map_block = False self.target_speed_map_sign = False if self.params.get("OpkrSafetyCamera", encoding="utf8") == "1": os.system("echo -n 0 > /data/params/d/OpkrSafetyCamera &") # Calculate speed for normal cruise control if enabled and not self.first_loop and not sm[ 'carState'].brakePressed and not sm['carState'].gasPressed: accel_limits = [ float(x) for x in calc_cruise_accel_limits(v_ego, following) ] jerk_limits = [ min(-0.1, accel_limits[0]), max(0.1, accel_limits[1]) ] # TODO: make a separate lookup for jerk tuning accel_limits_turns = limit_accel_in_turns( v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP) if force_slow_decel: # if required so, force a smooth deceleration accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL) accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1]) self.v_cruise, self.a_cruise = speed_smoother( self.v_acc_start, self.a_acc_start, v_cruise_setpoint, accel_limits_turns[1], accel_limits_turns[0], jerk_limits[1], jerk_limits[0], LON_MPC_STEP) # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) else: starting = long_control_state == LongCtrlState.starting a_ego = min(sm['carState'].aEgo, 0.0) reset_speed = self.CP.minSpeedCan if starting else v_ego reset_accel = self.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.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc1.update(sm['carState'], lead_1) self.mpc2.update(sm['carState'], lead_2) self.choose_solution(v_cruise_setpoint, enabled) # determine fcw if self.mpc1.new_lead: self.fcw_checker.reset_lead(cur_time) blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker self.fcw = self.fcw_checker.update( self.mpc1.mpc_solution, cur_time, sm['controlsState'].active, v_ego, sm['carState'].aEgo, lead_1.dRel, lead_1.vLead, lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw, blinkers) and not sm['carState'].brakePressed if self.fcw: cloudlog.info("FCW triggered %s", self.fcw_checker.counters) # Interpolate 0.05 seconds and save as starting point for next iteration a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * ( self.a_acc - self.a_acc_start) v_acc_sol = self.v_acc_start + CP.radarTimeStep * ( a_acc_sol + self.a_acc_start) / 2.0 self.v_acc_next = v_acc_sol self.a_acc_next = a_acc_sol self.first_loop = False
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
def update(self, rcv_times, CS, CP, VM, PP, live20, live100, md, live_map_data): """Gets called when new live20 is available""" cur_time = sec_since_boot() v_ego = CS.carState.vEgo gasbuttonstatus = CS.carState.gasbuttonstatus long_control_state = live100.live100.longControlState v_cruise_kph = live100.live100.vCruise force_slow_decel = live100.live100.forceDecel v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS for socket, event in self.poller.poll(0): if socket is self.lat_Control: self.lastlat_Control = messaging.recv_one(socket).latControl lead_1 = live20.live20.leadOne lead_2 = live20.live20.leadTwo enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0 v_speedlimit = NO_CURVATURE_SPEED v_curvature = NO_CURVATURE_SPEED v_speedlimit_ahead = NO_CURVATURE_SPEED map_age = cur_time - rcv_times['liveMapData'] map_valid = True #live_map_data.liveMapData.mapValid and map_age < 10.0 # Speed limit and curvature set_speed_limit_active = self.params.get( "LimitSetSpeed") == "1" and self.params.get( "SpeedLimitOffset") is not None if set_speed_limit_active and map_valid: offset = float(self.params.get("SpeedLimitOffset")) if live_map_data.liveMapData.speedLimitValid: speed_limit = live_map_data.liveMapData.speedLimit v_speedlimit = speed_limit + offset if live_map_data.liveMapData.speedLimitAheadValid and live_map_data.liveMapData.speedLimitAheadDistance < 200: speed_limit_ahead = live_map_data.liveMapData.speedLimitAhead #print "Speed Ahead found" #print speed_limit_ahead v_speedlimit_ahead = speed_limit_ahead + offset if live_map_data.liveMapData.curvatureValid: curvature = abs(live_map_data.liveMapData.curvature) a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph v_curvature = math.sqrt( a_y_max / max(1e-4, curvature)) / 1.3 * _brake_factor v_curvature = min(NO_CURVATURE_SPEED, v_curvature) decel_for_turn = bool( v_curvature < min([v_cruise_setpoint, v_speedlimit, v_ego + 1.])) v_cruise_setpoint = min( [v_cruise_setpoint, v_curvature, v_speedlimit, v_speedlimit_ahead]) # Calculate speed for normal cruise control if enabled: accel_limits = map( float, calc_cruise_accel_limits(v_ego, following, gasbuttonstatus)) if gasbuttonstatus == 0: accellimitmaxdynamic = -0.0018 * v_ego + 0.2 jerk_limits = [ min(-0.1, accel_limits[0]), max(accellimitmaxdynamic, accel_limits[1]) ] # dynamic elif gasbuttonstatus == 1: accellimitmaxsport = -0.002 * v_ego + 0.4 jerk_limits = [ min(-0.25, accel_limits[0]), max(accellimitmaxsport, accel_limits[1]) ] # sport elif gasbuttonstatus == 2: accellimitmaxeco = -0.0015 * v_ego + 0.1 jerk_limits = [ min(-0.1, accel_limits[0]), max(accellimitmaxeco, accel_limits[1]) ] # eco if not CS.carState.leftBlinker and not CS.carState.rightBlinker: steering_angle = CS.carState.steeringAngle if self.lastlat_Control and v_ego > 11: angle_later = self.lastlat_Control.anglelater else: angle_later = 0 else: angle_later = 0 steering_angle = 0 accel_limits = limit_accel_in_turns( v_ego, steering_angle, accel_limits, self.CP, angle_later * self.CP.steerRatio) if force_slow_decel: # if required so, force a smooth deceleration accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL) accel_limits[0] = min(accel_limits[0], accel_limits[1]) if v_speedlimit_ahead < v_speedlimit: time_to_speedlimit = max( 1.0, live_map_data.liveMapData.speedLimitAheadDistance / max(self.v_cruise, 1.)) #print "Decelerating in " #print time_to_speedlimit required_decel = min(0, (v_speedlimit_ahead - self.v_cruise) / time_to_speedlimit) * 5 if live_map_data.liveMapData.speedLimitAheadDistance < 100.0: max(required_decel * 10.0, -3.0) #print "required_decel" #print required_decel #print "accel_limits 0" #print accel_limits[0] #print "accel_limits 1" #print accel_limits[1] accel_limits[0] = min(accel_limits[0], required_decel) # Change accel limits based on time remaining to turn if decel_for_turn: time_to_turn = max( 1.0, live_map_data.liveMapData.distToTurn / max(self.v_cruise, 1.)) required_decel = min(0, (v_curvature - self.v_cruise) / time_to_turn) accel_limits[0] = max(accel_limits[0], required_decel) self.v_cruise, self.a_cruise = speed_smoother( self.v_acc_start, self.a_acc_start, v_cruise_setpoint, accel_limits[1], accel_limits[0], jerk_limits[1], jerk_limits[0], _DT_MPC) # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) else: starting = long_control_state == LongCtrlState.starting a_ego = min(CS.carState.aEgo, 0.0) reset_speed = MIN_CAN_SPEED if starting else v_ego reset_accel = self.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.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc1.update(CS, lead_1, v_cruise_setpoint) self.mpc2.update(CS, lead_2, v_cruise_setpoint) self.choose_solution(v_cruise_setpoint, enabled) # determine fcw if self.mpc1.new_lead: self.fcw_checker.reset_lead(cur_time) blinkers = CS.carState.leftBlinker or CS.carState.rightBlinker fcw = self.fcw_checker.update( self.mpc1.mpc_solution, cur_time, v_ego, CS.carState.aEgo, lead_1.dRel, lead_1.vLead, lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw, blinkers) and not CS.carState.brakePressed if fcw: cloudlog.info("FCW triggered %s", self.fcw_checker.counters) radar_dead = cur_time - rcv_times['live20'] > 0.5 radar_errors = list(live20.live20.radarErrors) radar_fault = car.RadarState.Error.fault in radar_errors radar_comm_issue = car.RadarState.Error.commIssue in radar_errors # **** send the plan **** plan_send = messaging.new_message() plan_send.init('plan') plan_send.plan.mdMonoTime = md.logMonoTime plan_send.plan.l20MonoTime = live20.logMonoTime # longitudal plan plan_send.plan.vCruise = self.v_cruise plan_send.plan.aCruise = self.a_cruise plan_send.plan.vStart = self.v_acc_start plan_send.plan.aStart = self.a_acc_start plan_send.plan.vTarget = self.v_acc plan_send.plan.aTarget = self.a_acc plan_send.plan.vTargetFuture = self.v_acc_future plan_send.plan.hasLead = self.mpc1.prev_lead_status plan_send.plan.hasrightLaneDepart = bool( PP.r_poly[3] > -1.1 and not CS.carState.rightBlinker) plan_send.plan.hasleftLaneDepart = bool( PP.l_poly[3] < 1.05 and not CS.carState.leftBlinker) plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource plan_send.plan.vCurvature = v_curvature plan_send.plan.decelForTurn = bool( decel_for_turn or v_speedlimit_ahead < min([v_speedlimit, v_ego + 1.])) plan_send.plan.mapValid = map_valid radar_valid = not (radar_dead or radar_fault) plan_send.plan.radarValid = bool(radar_valid) plan_send.plan.radarCommIssue = bool(radar_comm_issue) plan_send.plan.processingDelay = (plan_send.logMonoTime / 1e9) - rcv_times['live20'] # Send out fcw fcw = fcw and (self.fcw_enabled or long_control_state != LongCtrlState.off) plan_send.plan.fcw = fcw self.plan.send(plan_send.to_bytes()) # Interpolate 0.05 seconds and save as starting point for next iteration dt = 0.05 # s a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc - self.a_acc_start) v_acc_sol = self.v_acc_start + dt * (a_acc_sol + self.a_acc_start) / 2.0 self.v_acc_start = v_acc_sol self.a_acc_start = a_acc_sol
def update(self, sm, pm, CP, VM, PP, arne_sm): """Gets called when new radarState is available""" cur_time = sec_since_boot() # we read offset value every 5 seconds fixed_offset = 0.0 if not travis: fixed_offset = op_params.get('speed_offset', 0.0) if self.last_time > 5: try: self.offset = int( self.params.get("SpeedLimitOffset", encoding='utf8')) except (TypeError, ValueError): self.params.delete("SpeedLimitOffset") self.offset = 0 self.osm = self.params.get("LimitSetSpeed", encoding='utf8') == "1" self.last_time = 0 self.last_time = self.last_time + 1 gas_button_status = arne_sm['arne182Status'].gasbuttonstatus v_ego = sm['carState'].vEgo blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker if blinkers: steering_angle = sm['carState'].steeringAngle * 0.8 if v_ego < 11: angle_later = 0. else: angle_later = arne_sm['latControl'].anglelater * 0.8 else: steering_angle = sm['carState'].steeringAngle if v_ego < 11: angle_later = 0. else: angle_later = arne_sm['latControl'].anglelater long_control_state = sm['controlsState'].longControlState v_cruise_kph = sm['controlsState'].vCruise force_slow_decel = sm['controlsState'].forceDecel v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS lead_1 = sm['radarState'].leadOne lead_2 = sm['radarState'].leadTwo enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) following = (lead_1.status and lead_1.dRel < 40.0 and lead_1.vRel < 0.0) or (lead_2.status and lead_2.dRel < 40.0 and lead_2.vRel < 0.0) if gas_button_status == 1: speed_ahead_distance = 150 elif gas_button_status == 2: speed_ahead_distance = 350 else: speed_ahead_distance = default_brake_distance v_speedlimit = NO_CURVATURE_SPEED v_curvature_map = NO_CURVATURE_SPEED v_speedlimit_ahead = NO_CURVATURE_SPEED now = datetime.now() try: if sm['liveMapData'].speedLimitValid and osm and self.osm and ( sm['liveMapData'].lastGps.timestamp - time.mktime(now.timetuple()) * 1000) < 10000 and ( smart_speed or smart_speed_max_vego > v_ego): speed_limit = sm['liveMapData'].speedLimit if speed_limit is not None: v_speedlimit = speed_limit # offset is in percentage,. if v_ego > offset_limit: v_speedlimit = v_speedlimit * (1. + self.offset / 100.0) if v_speedlimit > fixed_offset: v_speedlimit = v_speedlimit + fixed_offset else: speed_limit = None if sm['liveMapData'].speedLimitAheadValid and osm and self.osm and sm[ 'liveMapData'].speedLimitAheadDistance < speed_ahead_distance and ( sm['liveMapData'].lastGps.timestamp - time.mktime(now.timetuple()) * 1000) < 10000 and ( smart_speed or smart_speed_max_vego > v_ego): distanceatlowlimit = 50 if sm['liveMapData'].speedLimitAhead < 21 / 3.6: distanceatlowlimit = speed_ahead_distance = ( v_ego - sm['liveMapData'].speedLimitAhead) * 3.6 * 2 if distanceatlowlimit < 50: distanceatlowlimit = 0 distanceatlowlimit = min(distanceatlowlimit, 100) speed_ahead_distance = ( v_ego - sm['liveMapData'].speedLimitAhead) * 3.6 * 5 speed_ahead_distance = min(speed_ahead_distance, 300) speed_ahead_distance = max(speed_ahead_distance, 50) if speed_limit is not None and sm[ 'liveMapData'].speedLimitAheadDistance > distanceatlowlimit and v_ego + 3 < sm[ 'liveMapData'].speedLimitAhead + ( speed_limit - sm['liveMapData'].speedLimitAhead ) * sm[ 'liveMapData'].speedLimitAheadDistance / speed_ahead_distance: speed_limit_ahead = sm['liveMapData'].speedLimitAhead + ( speed_limit - sm['liveMapData'].speedLimitAhead) * ( sm['liveMapData'].speedLimitAheadDistance - distanceatlowlimit) / (speed_ahead_distance - distanceatlowlimit) else: speed_limit_ahead = sm['liveMapData'].speedLimitAhead if speed_limit_ahead is not None: v_speedlimit_ahead = speed_limit_ahead if v_ego > offset_limit: v_speedlimit_ahead = v_speedlimit_ahead * ( 1. + self.offset / 100.0) if v_speedlimit_ahead > fixed_offset: v_speedlimit_ahead = v_speedlimit_ahead + fixed_offset if sm['liveMapData'].curvatureValid and sm[ 'liveMapData'].distToTurn < speed_ahead_distance and osm and self.osm and ( sm['liveMapData'].lastGps.timestamp - time.mktime(now.timetuple()) * 1000) < 10000: curvature = abs(sm['liveMapData'].curvature) radius = 1 / max(1e-4, curvature) * curvature_factor if gas_button_status == 1: radius = radius * 2.0 elif gas_button_status == 2: radius = radius * 1.0 else: radius = radius * 1.5 if radius > 500: c = 0.9 # 0.9 at 1000m = 108 kph elif radius > 250: c = 3.5 - 13 / 2500 * radius # 2.2 at 250m 84 kph else: c = 3.0 - 2 / 625 * radius # 3.0 at 15m 24 kph v_curvature_map = math.sqrt(c * radius) v_curvature_map = min(NO_CURVATURE_SPEED, v_curvature_map) except KeyError: pass decel_for_turn = bool(v_curvature_map < min( [v_cruise_setpoint, v_speedlimit, v_ego + 1.])) # Calculate speed for normal cruise control if enabled and not self.first_loop and not sm[ 'carState'].brakePressed and not sm['carState'].gasPressed: accel_limits = [ float(x) for x in calc_cruise_accel_limits( v_ego, following and (self.longitudinalPlanSource == 'mpc1' or self.longitudinalPlanSource == 'mpc2'), gas_button_status) ] jerk_limits = [ min(-0.1, accel_limits[0]), max(0.1, accel_limits[1]) ] # TODO: make a separate lookup for jerk tuning accel_limits_turns = limit_accel_in_turns(v_ego, steering_angle, accel_limits, self.CP, angle_later) if force_slow_decel: # if required so, force a smooth deceleration accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL) accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1]) if decel_for_turn and sm[ 'liveMapData'].distToTurn < speed_ahead_distance and not following: time_to_turn = max( 1.0, sm['liveMapData'].distToTurn / max( (v_ego + v_curvature_map) / 2, 1.)) required_decel = min(0, (v_curvature_map - v_ego) / time_to_turn) accel_limits[0] = max(accel_limits[0], required_decel) if v_speedlimit_ahead < v_speedlimit and v_ego > v_speedlimit_ahead and sm[ 'liveMapData'].speedLimitAheadDistance > 1.0 and not following: required_decel = min( 0, (v_speedlimit_ahead * v_speedlimit_ahead - v_ego * v_ego) / (sm['liveMapData'].speedLimitAheadDistance * 2)) required_decel = max(required_decel, -3.0) decel_for_turn = True accel_limits[0] = required_decel accel_limits[1] = required_decel self.a_acc_start = required_decel v_speedlimit_ahead = v_ego v_cruise_setpoint = min([ v_cruise_setpoint, v_curvature_map, v_speedlimit, v_speedlimit_ahead ]) #if (self.mpc1.prev_lead_status and self.mpc1.v_mpc < v_ego*0.99) or (self.mpc2.prev_lead_status and self.mpc2.v_mpc < v_ego*0.99): # v_cruise_setpoint = v_ego self.v_cruise, self.a_cruise = speed_smoother( self.v_acc_start, self.a_acc_start, v_cruise_setpoint, accel_limits_turns[1], accel_limits_turns[0], jerk_limits[1], jerk_limits[0], LON_MPC_STEP) # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) else: starting = long_control_state == LongCtrlState.starting a_ego = min(sm['carState'].aEgo, 0.0) reset_speed = MIN_CAN_SPEED if starting else v_ego reset_accel = self.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.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc_model.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc1.update(pm, sm['carState'], lead_1, v_cruise_setpoint) self.mpc2.update(pm, sm['carState'], lead_2, v_cruise_setpoint) self.mpc_model.update(sm['carState'].vEgo, sm['carState'].vEgo, sm['model'].longitudinal.distances, sm['model'].longitudinal.speeds, sm['model'].longitudinal.accelerations) self.choose_solution(v_cruise_setpoint, enabled, lead_1, lead_2, sm['carState'].steeringAngle) # determine fcw if self.mpc1.new_lead: self.fcw_checker.reset_lead(cur_time) fcw = self.fcw_checker.update( self.mpc1.mpc_solution, cur_time, sm['controlsState'].active, v_ego, sm['carState'].aEgo, lead_1.dRel, lead_1.vLead, lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw, blinkers) and not sm['carState'].brakePressed if fcw: cloudlog.info("FCW triggered %s", self.fcw_checker.counters) radar_dead = not sm.alive['radarState'] radar_errors = list(sm['radarState'].radarErrors) radar_fault = car.RadarData.Error.fault in radar_errors radar_can_error = car.RadarData.Error.canError in radar_errors # **** send the plan **** plan_send = messaging.new_message('plan') plan_send.valid = sm.all_alive_and_valid( service_list=['carState', 'controlsState', 'radarState']) plan_send.plan.mdMonoTime = sm.logMonoTime['model'] plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState'] # longitudal plan plan_send.plan.vCruise = float(self.v_cruise) plan_send.plan.aCruise = float(self.a_cruise) plan_send.plan.vStart = float(self.v_acc_start) plan_send.plan.aStart = float(self.a_acc_start) plan_send.plan.vTarget = float(self.v_acc) plan_send.plan.aTarget = float(self.a_acc) plan_send.plan.vTargetFuture = float(self.v_acc_future) plan_send.plan.hasLead = self.mpc1.prev_lead_status plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource plan_send.plan.vCurvature = float(v_curvature_map) plan_send.plan.decelForTurn = bool(decel_for_turn) plan_send.plan.mapValid = True radar_valid = not (radar_dead or radar_fault) plan_send.plan.radarValid = bool(radar_valid) plan_send.plan.radarCanError = bool(radar_can_error) plan_send.plan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState'] # Send out fcw plan_send.plan.fcw = fcw pm.send('plan', plan_send) # Interpolate 0.05 seconds and save as starting point for next iteration a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * ( self.a_acc - self.a_acc_start) v_acc_sol = self.v_acc_start + CP.radarTimeStep * ( a_acc_sol + self.a_acc_start) / 2.0 self.v_acc_start = v_acc_sol self.a_acc_start = a_acc_sol self.first_loop = False
def update(self, sm, CP): """Gets called when new radarState is available""" cur_time = sec_since_boot() try: input_event = self.input_queue.get_nowait() if input_event == InputEvent.LONG_PRESS: self.TR_override = 1.8 if self.TR_override is None else None # Output current status output_event = OutputEvent.LONG_DIM if self.TR_override is None else OutputEvent.SHORT_DIM try: self.output_queue.put_nowait(output_event) except Full: log_f = open('/data/openpilot-patch/mpc_update_log.txt', 'a') log_f.write(f"{datetime.now()} Output queue is full\n") log_f.flush() subprocess.Popen( ['python', '/data/openpilot-patch/util/error.py']) except TimeoutError: pass except Empty: pass v_ego = sm['carState'].vEgo long_control_state = sm['controlsState'].longControlState v_cruise_kph = sm['controlsState'].vCruise force_slow_decel = sm['controlsState'].forceDecel v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX) v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS lead_1 = sm['radarState'].leadOne lead_2 = sm['radarState'].leadTwo enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0 self.v_acc_start = self.v_acc_next self.a_acc_start = self.a_acc_next # Calculate speed for normal cruise control if enabled and not self.first_loop and not sm['carState'].gasPressed: accel_limits = [ float(x) for x in calc_cruise_accel_limits(v_ego, following) ] jerk_limits = [ min(-0.1, accel_limits[0]), max(0.1, accel_limits[1]) ] # TODO: make a separate lookup for jerk tuning accel_limits_turns = limit_accel_in_turns( v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP) if force_slow_decel: # if required so, force a smooth deceleration accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL) accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1]) self.v_cruise, self.a_cruise = speed_smoother( self.v_acc_start, self.a_acc_start, v_cruise_setpoint, accel_limits_turns[1], accel_limits_turns[0], jerk_limits[1], jerk_limits[0], LON_MPC_STEP) # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) else: starting = long_control_state == LongCtrlState.starting a_ego = min(sm['carState'].aEgo, 0.0) reset_speed = self.CP.minSpeedCan if starting else v_ego reset_accel = self.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.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc1.update(sm['carState'], lead_1, self.TR_override) self.mpc2.update(sm['carState'], lead_2, self.TR_override) self.choose_solution(v_cruise_setpoint, enabled) # determine fcw if self.mpc1.new_lead: self.fcw_checker.reset_lead(cur_time) blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker self.fcw = self.fcw_checker.update( self.mpc1.mpc_solution, cur_time, sm['controlsState'].active, v_ego, sm['carState'].aEgo, lead_1.dRel, lead_1.vLead, lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw, blinkers) and not sm['carState'].brakePressed if self.fcw: cloudlog.info("FCW triggered %s", self.fcw_checker.counters) # Interpolate 0.05 seconds and save as starting point for next iteration a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * ( self.a_acc - self.a_acc_start) v_acc_sol = self.v_acc_start + CP.radarTimeStep * ( a_acc_sol + self.a_acc_start) / 2.0 self.v_acc_next = v_acc_sol self.a_acc_next = a_acc_sol self.first_loop = False
def run_following_distance_simulation(TR_override, v_lead, t_end=200.0): dt = 0.2 t = 0. lead_pos = LeadPos(v_lead) x_ego = 0.0 v_ego = v_lead a_ego = 0.0 v_cruise_setpoint = v_lead + 10. pm = FakePubMaster() mpc = LongitudinalMpc(1, TR_override) datapoints = [{'t': t, 'x_ego': x_ego, 'x_lead': lead_pos.x}] first = True while t < t_end: # Run cruise control accel_limits = [ float(x) for x in calc_cruise_accel_limits(v_ego, False) ] jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] v_cruise, a_cruise = speed_smoother(v_ego, a_ego, v_cruise_setpoint, accel_limits[1], accel_limits[0], jerk_limits[1], jerk_limits[0], dt) # Setup CarState CS = messaging.new_message('carState') CS.carState.vEgo = v_ego CS.carState.aEgo = a_ego # Setup lead packet lead = log.RadarState.LeadData.new_message() lead.status = True lead.dRel = lead_pos.x - x_ego lead.vLead = lead_pos.v lead.aLeadK = 0.0 # Run MPC mpc.set_cur_state(v_ego, a_ego) if first: # Make sure MPC is converged on first timestep for _ in range(20): mpc.update(CS.carState, lead) mpc.publish(pm) mpc.update(CS.carState, lead) mpc.publish(pm) # Choose slowest of two solutions if v_cruise < mpc.v_mpc: v_ego, a_ego = v_cruise, a_cruise else: v_ego, a_ego = mpc.v_mpc, mpc.a_mpc # Update state lead_pos.update(t, dt) x_ego += v_ego * dt t += dt first = False datapoints.append({'t': t, 'x_ego': x_ego, 'x_lead': lead_pos.x}) filename = f'test_out/{v_lead}_{TR_override}.json' with open(filename, 'w') as datafile: json.dump(datapoints, datafile) return lead_pos.x - x_ego