selfdrive/controls/lib/planner.py
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
    a_ego = sm['carState'].aEgo

    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, self.op_params)]
      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 self.op_params.get(ENABLE_COASTING):
        self.coast_speed = self.op_params.get(COAST_SPEED) * CV.MPH_TO_MS
        self.v_cruise, self.a_cruise = self.choose_cruise(sm,
                                                          v_ego,
                                                          a_ego,
                                                          v_cruise_setpoint,
                                                          accel_limits_turns,
                                                          jerk_limits)
      else:
        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(a_ego, 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.cruise_plan = Source.cruiseGas

    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
Gets called when new radarState is available
Gets called when new radarState is available
Gets called when new radarState is available
def update(self, sm, pm, CP, VM, PP):
    cur_time = sec_since_boot()
    v_ego = sm['carState'].vEgo
    a_ego = sm['carState'].aEgo
    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
    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, self.op_params)]
      jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])]  
      accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP)
      if force_slow_decel:
        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 self.op_params.get(ENABLE_COASTING):
        self.coast_speed = self.op_params.get(COAST_SPEED) * CV.MPH_TO_MS
        self.v_cruise, self.a_cruise = self.choose_cruise(sm,
                                                          v_ego,
                                                          a_ego,
                                                          v_cruise_setpoint,
                                                          accel_limits_turns,
                                                          jerk_limits)
      else:
        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_cruise = max(self.v_cruise, 0.)
    else:
      starting = long_control_state == LongCtrlState.starting
      a_ego = min(a_ego, 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.cruise_plan = Source.cruiseGas
    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)
    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
    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']
    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']
    plan_send.plan.fcw = fcw
    pm.send('plan', plan_send)
    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 ) : cur_time = sec_since_boot ( ) v_ego = sm [ 'carState' ] . vEgo a_ego = sm [ 'carState' ] . aEgo 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 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 , self . op_params ) ] jerk_limits = [ min ( - 0.1 , accel_limits [ 0 ] ) , max ( 0.1 , accel_limits [ 1 ] ) ] accel_limits_turns = limit_accel_in_turns ( v_ego , sm [ 'carState' ] . steeringAngle , accel_limits , self . CP ) if force_slow_decel : 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 self . op_params . get ( ENABLE_COASTING ) : self . coast_speed = self . op_params . get ( COAST_SPEED ) * CV . MPH_TO_MS self . v_cruise , self . a_cruise = self . choose_cruise ( sm , v_ego , a_ego , v_cruise_setpoint , accel_limits_turns , jerk_limits ) else : 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_cruise = max ( self . v_cruise , 0. ) else : starting = long_control_state == LongCtrlState . starting a_ego = min ( a_ego , 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 . cruise_plan = Source . cruiseGas 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 ) 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 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' ] 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' ] plan_send . plan . fcw = fcw pm . send ( 'plan' , plan_send ) 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
Gets called when new radarState is available
Gets called when new radarState is available
"""Gets called when new radarState is available""" # Calculate speed for normal cruise control # TODO: make a separate lookup for jerk tuning # if required so, force a smooth deceleration # cruise speed can't be negative even is user is distracted # determine fcw # **** send the plan **** # longitudal plan # Send out fcw # Interpolate 0.05 seconds and save as starting point for next iteration
self
sm
pm
CP
VM
PP