selfdrive/controls/lib/longcontrol.py
def update(self, active, CS, v_target, v_target_future, a_target, CP, source):
    """Update longitudinal control. This updates the state machine and runs a PID loop"""
    # Actuation limits
    gm_bp = CP.gasMaxBP
    gm_v = CP.gasMaxV
    bm_bp = CP.brakeMaxBP
    bm_v = CP.brakeMaxV
    dz_bp = CP.longitudinalTuning.deadzoneBP
    dz_v = CP.longitudinalTuning.deadzoneV

    if self.op_params.get(ENABLE_LONG_PARAMS):
      if self.op_params.get(ENABLE_GAS_PARAMS):
        gm_bp = self.op_params.get(GAS_MAX_BP)
        gm_v = self.op_params.get(GAS_MAX_V)
      if self.op_params.get(ENABLE_BRAKE_PARAMS):
        bm_bp = self.op_params.get(BRAKE_MAX_BP)
        bm_v = self.op_params.get(BRAKE_MAX_V)
      if self.op_params.get(ENABLE_LONG_DEADZONE_PARAMS):
        dz_bp = self.op_params.get(LONG_DEADZONE_BP)
        dz_v = self.op_params.get(LONG_DEADZONE_V)

    gas_max = interp(CS.vEgo, gm_bp, gm_v)
    brake_max = interp(CS.vEgo, bm_bp, bm_v)

    # Update state machine
    output_gb = self.last_output_gb
    self.long_control_state = long_control_state_trans(active, self.long_control_state, CS.vEgo,
                                                       v_target_future, self.v_pid, output_gb,
                                                       CS.brakePressed, CS.cruiseState.standstill)

    v_ego_pid = max(CS.vEgo, MIN_CAN_SPEED)  # Without this we get jumps, CAN bus reports 0 when speed < 0.3

    if self.long_control_state == LongCtrlState.off or CS.gasPressed:
      self.reset(v_ego_pid)
      output_gb = 0.

    # tracking objects and driving
    elif self.long_control_state == LongCtrlState.pid:
      self.v_pid = v_target
      self.pid.pos_limit = gas_max
      self.pid.neg_limit = - brake_max

      if self.op_params.get(ENABLE_COASTING) and self.op_params.get(EVAL_COAST_LONG):
        no_gas = source in [Source.cruiseBrake, Source.cruiseCoast]
        no_brake = source in [Source.cruiseGas, Source.cruiseCoast]

        if no_gas:
          self.pid.pos_limit = 0.

        if no_brake:
          self.pid.neg_limit = 0.

        if no_gas and no_brake:
          self.reset(CS.vEgo)

      # Toyota starts braking more when it thinks you want to stop
      # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
      prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7
      deadzone = interp(v_ego_pid, dz_bp, dz_v)

      output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot)

      if prevent_overshoot:
        output_gb = min(output_gb, 0.0)

    # Intention is to stop, switch to a different brake control until we stop
    elif self.long_control_state == LongCtrlState.stopping:
      # Keep applying brakes until the car is stopped
      if not CS.standstill or output_gb > -BRAKE_STOPPING_TARGET:
        output_gb -= STOPPING_BRAKE_RATE / RATE
      output_gb = clip(output_gb, -brake_max, gas_max)

      self.reset(CS.vEgo)

    # Intention is to move again, release brake fast before handing control to PID
    elif self.long_control_state == LongCtrlState.starting:
      if output_gb < -0.2:
        output_gb += STARTING_BRAKE_RATE / RATE
      self.reset(CS.vEgo)

    self.last_output_gb = output_gb
    final_gas = clip(output_gb, 0., gas_max)
    final_brake = -clip(output_gb, -brake_max, 0.)

    return final_gas, final_brake
Update longitudinal control. This updates the state machine and runs a PID loop
Update longitudinal control. This updates the state machine and runs a PID loop
Update longitudinal control . This updates the state machine and runs a PID loop
def update(self, active, CS, v_target, v_target_future, a_target, CP, source):
    gm_bp = CP.gasMaxBP
    gm_v = CP.gasMaxV
    bm_bp = CP.brakeMaxBP
    bm_v = CP.brakeMaxV
    dz_bp = CP.longitudinalTuning.deadzoneBP
    dz_v = CP.longitudinalTuning.deadzoneV
    if self.op_params.get(ENABLE_LONG_PARAMS):
      if self.op_params.get(ENABLE_GAS_PARAMS):
        gm_bp = self.op_params.get(GAS_MAX_BP)
        gm_v = self.op_params.get(GAS_MAX_V)
      if self.op_params.get(ENABLE_BRAKE_PARAMS):
        bm_bp = self.op_params.get(BRAKE_MAX_BP)
        bm_v = self.op_params.get(BRAKE_MAX_V)
      if self.op_params.get(ENABLE_LONG_DEADZONE_PARAMS):
        dz_bp = self.op_params.get(LONG_DEADZONE_BP)
        dz_v = self.op_params.get(LONG_DEADZONE_V)
    gas_max = interp(CS.vEgo, gm_bp, gm_v)
    brake_max = interp(CS.vEgo, bm_bp, bm_v)
    output_gb = self.last_output_gb
    self.long_control_state = long_control_state_trans(active, self.long_control_state, CS.vEgo,
                                                       v_target_future, self.v_pid, output_gb,
                                                       CS.brakePressed, CS.cruiseState.standstill)
    v_ego_pid = max(CS.vEgo, MIN_CAN_SPEED)  
    if self.long_control_state == LongCtrlState.off or CS.gasPressed:
      self.reset(v_ego_pid)
      output_gb = 0.
    elif self.long_control_state == LongCtrlState.pid:
      self.v_pid = v_target
      self.pid.pos_limit = gas_max
      self.pid.neg_limit = - brake_max
      if self.op_params.get(ENABLE_COASTING) and self.op_params.get(EVAL_COAST_LONG):
        no_gas = source in [Source.cruiseBrake, Source.cruiseCoast]
        no_brake = source in [Source.cruiseGas, Source.cruiseCoast]
        if no_gas:
          self.pid.pos_limit = 0.
        if no_brake:
          self.pid.neg_limit = 0.
        if no_gas and no_brake:
          self.reset(CS.vEgo)
      prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7
      deadzone = interp(v_ego_pid, dz_bp, dz_v)
      output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot)
      if prevent_overshoot:
        output_gb = min(output_gb, 0.0)
    elif self.long_control_state == LongCtrlState.stopping:
      if not CS.standstill or output_gb > -BRAKE_STOPPING_TARGET:
        output_gb -= STOPPING_BRAKE_RATE / RATE
      output_gb = clip(output_gb, -brake_max, gas_max)
      self.reset(CS.vEgo)
    elif self.long_control_state == LongCtrlState.starting:
      if output_gb < -0.2:
        output_gb += STARTING_BRAKE_RATE / RATE
      self.reset(CS.vEgo)
    self.last_output_gb = output_gb
    final_gas = clip(output_gb, 0., gas_max)
    final_brake = -clip(output_gb, -brake_max, 0.)
    return final_gas, final_brake
def update ( self , active , CS , v_target , v_target_future , a_target , CP , source ) : gm_bp = CP . gasMaxBP gm_v = CP . gasMaxV bm_bp = CP . brakeMaxBP bm_v = CP . brakeMaxV dz_bp = CP . longitudinalTuning . deadzoneBP dz_v = CP . longitudinalTuning . deadzoneV if self . op_params . get ( ENABLE_LONG_PARAMS ) : if self . op_params . get ( ENABLE_GAS_PARAMS ) : gm_bp = self . op_params . get ( GAS_MAX_BP ) gm_v = self . op_params . get ( GAS_MAX_V ) if self . op_params . get ( ENABLE_BRAKE_PARAMS ) : bm_bp = self . op_params . get ( BRAKE_MAX_BP ) bm_v = self . op_params . get ( BRAKE_MAX_V ) if self . op_params . get ( ENABLE_LONG_DEADZONE_PARAMS ) : dz_bp = self . op_params . get ( LONG_DEADZONE_BP ) dz_v = self . op_params . get ( LONG_DEADZONE_V ) gas_max = interp ( CS . vEgo , gm_bp , gm_v ) brake_max = interp ( CS . vEgo , bm_bp , bm_v ) output_gb = self . last_output_gb self . long_control_state = long_control_state_trans ( active , self . long_control_state , CS . vEgo , v_target_future , self . v_pid , output_gb , CS . brakePressed , CS . cruiseState . standstill ) v_ego_pid = max ( CS . vEgo , MIN_CAN_SPEED ) if self . long_control_state == LongCtrlState . off or CS . gasPressed : self . reset ( v_ego_pid ) output_gb = 0. elif self . long_control_state == LongCtrlState . pid : self . v_pid = v_target self . pid . pos_limit = gas_max self . pid . neg_limit = - brake_max if self . op_params . get ( ENABLE_COASTING ) and self . op_params . get ( EVAL_COAST_LONG ) : no_gas = source in [ Source . cruiseBrake , Source . cruiseCoast ] no_brake = source in [ Source . cruiseGas , Source . cruiseCoast ] if no_gas : self . pid . pos_limit = 0. if no_brake : self . pid . neg_limit = 0. if no_gas and no_brake : self . reset ( CS . vEgo ) prevent_overshoot = not CP . stoppingControl and CS . vEgo < 1.5 and v_target_future < 0.7 deadzone = interp ( v_ego_pid , dz_bp , dz_v ) output_gb = self . pid . update ( self . v_pid , v_ego_pid , speed = v_ego_pid , deadzone = deadzone , feedforward = a_target , freeze_integrator = prevent_overshoot ) if prevent_overshoot : output_gb = min ( output_gb , 0.0 ) elif self . long_control_state == LongCtrlState . stopping : if not CS . standstill or output_gb > - BRAKE_STOPPING_TARGET : output_gb -= STOPPING_BRAKE_RATE / RATE output_gb = clip ( output_gb , - brake_max , gas_max ) self . reset ( CS . vEgo ) elif self . long_control_state == LongCtrlState . starting : if output_gb < - 0.2 : output_gb += STARTING_BRAKE_RATE / RATE self . reset ( CS . vEgo ) self . last_output_gb = output_gb final_gas = clip ( output_gb , 0. , gas_max ) final_brake = - clip ( output_gb , - brake_max , 0. ) return final_gas , final_brake
Update longitudinal control.
Update longitudinal control .
"""Update longitudinal control. This updates the state machine and runs a PID loop""" # Actuation limits # Update state machine # Without this we get jumps, CAN bus reports 0 when speed < 0.3 # tracking objects and driving # Toyota starts braking more when it thinks you want to stop # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration # Intention is to stop, switch to a different brake control until we stop # Keep applying brakes until the car is stopped # Intention is to move again, release brake fast before handing control to PID
self
active
CS
v_target
v_target_future
a_target
CP
source