def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_alert, left_line, right_line, lead, left_lane_depart, right_lane_depart): # *** compute control surfaces *** # gas and brake if CS.CP.enableGasInterceptor and enabled: MAX_INTERCEPTOR_GAS = 0.5 # RAV4 has very sensitive gas pedal if CS.CP.carFingerprint in [CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH]: PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0]) elif CS.CP.carFingerprint in [CAR.COROLLA]: PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0]) else: PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0]) # offset for creep and windbrake pedal_offset = interp(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-.4, 0.0, 0.2]) pedal_command = PEDAL_SCALE * (actuators.accel + pedal_offset) interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS) else: interceptor_gas_cmd = 0. pcm_accel_cmd = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) # steer torque new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX)) apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, CarControllerParams) self.steer_rate_limited = new_steer != apply_steer # Cut steering while we're in a known fault state (2s) if not enabled or CS.steer_state in [9, 25]: apply_steer = 0 apply_steer_req = 0 else: apply_steer_req = 1 # TODO: probably can delete this. CS.pcm_acc_status uses a different signal # than CS.cruiseState.enabled. confirm they're not meaningfully different if not enabled and CS.pcm_acc_status: pcm_cancel_cmd = 1 # on entering standstill, send standstill request if CS.out.standstill and not self.last_standstill and CS.CP.carFingerprint not in NO_STOP_TIMER_CAR: self.standstill_req = True if CS.pcm_acc_status != 8: # pcm entered standstill or it's disabled self.standstill_req = False self.last_steer = apply_steer self.last_standstill = CS.out.standstill can_sends = [] #*** control msgs *** #print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor) # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2; # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed # on consecutive messages can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame)) if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR: can_sends.append(create_lta_steer_command(self.packer, 0, 0, frame // 2)) # LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda # if frame % 2 == 0: # can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2)) # can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, frame // 2)) # we can spam can to cancel the system even if we are using lat only control if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or pcm_cancel_cmd: lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present do ACC can be engaged # Lexus IS uses a different cancellation message if pcm_cancel_cmd and CS.CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_RC]: can_sends.append(create_acc_cancel_command(self.packer)) elif CS.CP.openpilotLongitudinalControl: can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type)) else: can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type)) if frame % 2 == 0 and CS.CP.enableGasInterceptor: # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd. # This prevents unexpected pedal range rescaling can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, frame // 2)) # ui mesg is at 100Hz but we send asap if: # - there is something to display # - there is something to stop displaying fcw_alert = hud_alert == VisualAlert.fcw steer_alert = hud_alert in [VisualAlert.steerRequired, VisualAlert.ldw] send_ui = False if ((fcw_alert or steer_alert) and not self.alert_active) or \ (not (fcw_alert or steer_alert) and self.alert_active): send_ui = True self.alert_active = not self.alert_active elif pcm_cancel_cmd: # forcing the pcm to disengage causes a bad fault sound so play a good sound instead send_ui = True if (frame % 100 == 0 or send_ui): can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart, enabled)) if frame % 100 == 0 and CS.CP.enableDsu: can_sends.append(create_fcw_command(self.packer, fcw_alert)) #*** static msgs *** for (addr, cars, bus, fr_step, vl) in STATIC_DSU_MSGS: if frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars: can_sends.append(make_can_msg(addr, vl, bus)) return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, left_line, right_line, lead, left_lane_depart, right_lane_depart): # *** compute control surfaces *** # gas and brake interceptor_gas_cmd = 0. pcm_accel_cmd = actuators.gas - actuators.brake if CS.CP.enableGasInterceptor: # handle hysteresis when around the minimum acc speed if CS.out.vEgo < MIN_ACC_SPEED: self.use_interceptor = True elif CS.out.vEgo > MIN_ACC_SPEED + PEDAL_HYST_GAP: self.use_interceptor = False if self.use_interceptor and enabled: # only send negative accel when using interceptor. gas handles acceleration # +0.06 offset to reduce ABS pump usage when OP is engaged interceptor_gas_cmd = clip(actuators.gas, 0., 1.) pcm_accel_cmd = 0.06 - actuators.brake pcm_accel_cmd, self.accel_steady = accel_hysteresis( pcm_accel_cmd, self.accel_steady, enabled) pcm_accel_cmd = clip(pcm_accel_cmd * CarControllerParams.ACCEL_SCALE, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) # steer torque new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX)) apply_steer = apply_toyota_steer_torque_limits( new_steer, self.last_steer, CS.out.steeringTorqueEps, CarControllerParams) self.steer_rate_limited = new_steer != apply_steer # Cut steering while we're in a known fault state (2s) if not enabled or CS.steer_state in [9, 25]: apply_steer = 0 apply_steer_req = 0 else: apply_steer_req = 1 if not enabled and CS.pcm_acc_status: # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated pcm_cancel_cmd = 1 # on entering standstill, send standstill request if CS.out.standstill and not self.last_standstill and CS.CP.carFingerprint not in NO_STOP_TIMER_CAR: self.standstill_req = True if CS.pcm_acc_status != 8: # pcm entered standstill or it's disabled self.standstill_req = False self.last_steer = apply_steer self.last_accel = pcm_accel_cmd self.last_standstill = CS.out.standstill can_sends = [] #*** control msgs *** #print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor) # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2; # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed # on consecutive messages can_sends.append( create_steer_command(self.packer, apply_steer, apply_steer_req, frame)) if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR: can_sends.append( create_lta_steer_command(self.packer, 0, 0, frame // 2)) # LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda # if frame % 2 == 0: # can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2)) # can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, frame // 2)) # we can spam can to cancel the system even if we are using lat only control if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or pcm_cancel_cmd: lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present do ACC can be engaged # Lexus IS uses a different cancellation message if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS: can_sends.append(create_acc_cancel_command(self.packer)) elif CS.CP.openpilotLongitudinalControl: can_sends.append( create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type)) else: can_sends.append( create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type)) if frame % 2 == 0 and CS.CP.enableGasInterceptor: # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd. # This prevents unexpected pedal range rescaling can_sends.append( create_gas_command(self.packer, interceptor_gas_cmd, frame // 2)) # ui mesg is at 100Hz but we send asap if: # - there is something to display # - there is something to stop displaying fcw_alert = hud_alert == VisualAlert.fcw steer_alert = hud_alert in [VisualAlert.steerRequired, VisualAlert.ldw] send_ui = False if ((fcw_alert or steer_alert) and not self.alert_active) or \ (not (fcw_alert or steer_alert) and self.alert_active): send_ui = True self.alert_active = not self.alert_active elif pcm_cancel_cmd: # forcing the pcm to disengage causes a bad fault sound so play a good sound instead send_ui = True if (frame % 100 == 0 or send_ui): can_sends.append( create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart)) if frame % 100 == 0 and CS.CP.enableDsu: can_sends.append(create_fcw_command(self.packer, fcw_alert)) #*** static msgs *** for (addr, cars, bus, fr_step, vl) in STATIC_DSU_MSGS: if frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars: can_sends.append(make_can_msg(addr, vl, bus)) return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, left_line, right_line, lead, left_lane_depart, right_lane_depart): # *** compute control surfaces *** self.sm.update(0) if self.sm.updated['radarState']: self.lead_v = self.sm['radarState'].leadOne.vRel self.lead_a = self.sm['radarState'].leadOne.aRel self.lead_d = self.sm['radarState'].leadOne.dRel if self.sm.updated['controlsState']: self.LCS = self.sm['controlsState'].longControlState if frame % 500 == 0: # *** 5 sec interval? *** print(self.LCS) # gas and brake # apply_gas = 0. # apply_accel = actuators.gas - actuators.brake # if CS.CP.enableGasInterceptor and enabled and CS.out.vEgo < MIN_ACC_SPEED: # apply_gas = clip(actuators.gas, 0., 1.) # # converts desired acceleration to gas percentage for pedal # # +0.06 offset to reduce ABS pump usage when applying very small gas # if apply_accel * CarControllerParams.ACCEL_SCALE > coast_accel(CS.out.vEgo): # apply_gas = clip(compute_gb_pedal(apply_accel * CarControllerParams.ACCEL_SCALE, CS.out.vEgo), 0., 1.) # apply_accel += 0.06 apply_gas = clip(actuators.gas, 0., 1.) if CS.CP.enableGasInterceptor: # send only negative accel if interceptor is detected. otherwise, send the regular value # +0.06 offset to reduce ABS pump usage when OP is engaged #apply_accel = 0.06 - actuators.brake # Original if lead: apply_accel = 0.06 - actuators.brake #apply_accel = 0.06 else: apply_accel = 0.06 # End new else: apply_accel = actuators.gas - actuators.brake apply_accel, self.accel_steady = accel_hysteresis( apply_accel, self.accel_steady, enabled) apply_accel = clip(apply_accel * CarControllerParams.ACCEL_SCALE, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) # steer torque new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX)) apply_steer = apply_toyota_steer_torque_limits( new_steer, self.last_steer, CS.out.steeringTorqueEps, CarControllerParams) self.steer_rate_limited = new_steer != apply_steer # Cut steering while we're in a known fault state (2s) if not enabled or abs(CS.out.steeringRateDeg) > 100: #if not enabled or CS.steer_state in [9, 25] or CS.out.epsDisabled==1 or abs(CS.out.steeringRateDeg) > 100: #Original statement apply_steer = 0 apply_steer_req = 0 else: apply_steer_req = 1 #if not enabled and CS.pcm_acc_status: # Original if not enabled: # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated pcm_cancel_cmd = 1 # on entering standstill, send standstill request if CS.out.standstill and not self.last_standstill and CS.CP.carFingerprint not in NO_STOP_TIMER_CAR and not self.standstill_hack: self.standstill_req = True if CS.pcm_acc_status != 8: # pcm entered standstill or it's disabled self.standstill_req = False self.last_steer = apply_steer self.last_accel = apply_accel self.last_standstill = CS.out.standstill can_sends = [] if (frame % 2 == 0): can_sends.append( create_lead_command(self.packer, self.lead_v, self.lead_a, self.lead_d)) #*** control msgs *** #print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor) # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2; # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed # on consecutive messages if Ecu.fwdCamera in self.fake_ecus: can_sends.append( create_steer_command(self.packer, apply_steer, apply_steer_req, frame)) if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR: can_sends.append( create_lta_steer_command(self.packer, 0, 0, frame // 2)) # LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda # if frame % 2 == 0: # can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2)) # can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, frame // 2)) # we can spam can to cancel the system even if we are using lat only control if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or ( pcm_cancel_cmd and Ecu.fwdCamera in self.fake_ecus): lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present do ACC can be engaged # Lexus IS uses a different cancellation message if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS: can_sends.append(create_acc_cancel_command(self.packer)) elif CS.CP.openpilotLongitudinalControl: can_sends.append( create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, lead)) else: #can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead)) # Original value can_sends.append( create_accel_command(self.packer, 500, pcm_cancel_cmd, False, lead)) if (frame % 2 == 0) and (CS.CP.enableGasInterceptor): # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas. # This prevents unexpected pedal range rescaling can_sends.append( create_gas_command(self.packer, apply_gas, frame // 2)) # ui mesg is at 100Hz but we send asap if: # - there is something to display # - there is something to stop displaying fcw_alert = hud_alert == VisualAlert.fcw steer_alert = hud_alert == VisualAlert.steerRequired send_ui = False if ((fcw_alert or steer_alert) and not self.alert_active) or \ (not (fcw_alert or steer_alert) and self.alert_active): send_ui = True self.alert_active = not self.alert_active elif pcm_cancel_cmd: # forcing the pcm to disengage causes a bad fault sound so play a good sound instead send_ui = True if (frame % 100 == 0 or send_ui) and Ecu.fwdCamera in self.fake_ecus: can_sends.append( create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart)) if frame % 100 == 0 and Ecu.dsu in self.fake_ecus: can_sends.append(create_fcw_command(self.packer, fcw_alert)) #*** static msgs *** for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS: if frame % fr_step == 0 and ecu in self.fake_ecus and CS.CP.carFingerprint in cars: can_sends.append(make_can_msg(addr, vl, bus)) return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, left_line, right_line, lead, left_lane_depart, right_lane_depart, dragonconf): # *** compute control surfaces *** # gas and brake interceptor_gas_cmd = 0. pcm_accel_cmd = actuators.accel if CS.CP.enableGasInterceptor: # handle hysteresis when around the minimum acc speed if CS.out.vEgo < MIN_ACC_SPEED: self.use_interceptor = True elif CS.out.vEgo > MIN_ACC_SPEED + PEDAL_HYST_GAP: self.use_interceptor = False if self.use_interceptor and enabled: # only send negative accel when using interceptor. gas handles acceleration # +0.18 m/s^2 offset to reduce ABS pump usage when OP is engaged MAX_INTERCEPTOR_GAS = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED], [0.2, 0.5]) interceptor_gas_cmd = clip(actuators.accel / PEDAL_SCALE, 0., MAX_INTERCEPTOR_GAS) pcm_accel_cmd = 0.18 - max(0, -actuators.accel) pcm_accel_cmd, self.accel_steady = accel_hysteresis( pcm_accel_cmd, self.accel_steady, enabled) pcm_accel_cmd = clip(pcm_accel_cmd, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) # steer torque new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX)) apply_steer = apply_toyota_steer_torque_limits( new_steer, self.last_steer, CS.out.steeringTorqueEps, CarControllerParams) self.steer_rate_limited = new_steer != apply_steer # Cut steering while we're in a known fault state (2s) if not enabled or CS.steer_state in [ 9, 25 ] or abs(CS.out.steeringRateDeg) > 100 or ( abs(CS.out.steeringAngleDeg) > 150 and CS.CP.carFingerprint in [CAR.RAV4H, CAR.PRIUS]): apply_steer = 0 apply_steer_req = 0 else: apply_steer_req = 1 # TODO: probably can delete this. CS.pcm_acc_status uses a different signal # than CS.cruiseState.enabled. confirm they're not meaningfully different if not enabled and CS.pcm_acc_status: pcm_cancel_cmd = 1 # on entering standstill, send standstill request if not dragonconf.dpToyotaSng and CS.out.standstill and not self.last_standstill and CS.CP.carFingerprint not in NO_STOP_TIMER_CAR: self.standstill_req = True if CS.pcm_acc_status != 8: # pcm entered standstill or it's disabled self.standstill_req = False # dp blinker_on = CS.out.leftBlinker or CS.out.rightBlinker if not enabled: self.blinker_end_frame = 0 if self.last_blinker_on and not blinker_on: self.blinker_end_frame = frame + dragonconf.dpSignalOffDelay apply_steer = common_controller_ctrl( enabled, dragonconf, blinker_on or frame < self.blinker_end_frame, apply_steer, CS.out.vEgo) self.last_blinker_on = blinker_on self.last_steer = apply_steer self.last_accel = pcm_accel_cmd self.last_standstill = CS.out.standstill can_sends = [] #*** control msgs *** #print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor) # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2; # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed # on consecutive messages can_sends.append( create_steer_command(self.packer, apply_steer, apply_steer_req, frame)) if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR: can_sends.append( create_lta_steer_command(self.packer, 0, 0, frame // 2)) # LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda # if frame % 2 == 0: # can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2)) # can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, frame // 2)) if dragonconf.dpAtl and dragonconf.dpAtlOpLong and not CS.out.cruiseActualEnabled: pcm_accel_cmd = 0. if CS.CP.enableGasInterceptor: interceptor_gas_cmd = 0. # we can spam can to cancel the system even if we are using lat only control if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or pcm_cancel_cmd: lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present do ACC can be engaged if dragonconf.dpAtl and not dragonconf.dpAtlOpLong: pass # Lexus IS uses a different cancellation message elif pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS: can_sends.append(create_acc_cancel_command(self.packer)) elif CS.CP.openpilotLongitudinalControl: can_sends.append( create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, CS.distance)) else: can_sends.append( create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, CS.distance)) if frame % 2 == 0 and CS.CP.enableGasInterceptor: # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd. # This prevents unexpected pedal range rescaling can_sends.append( create_gas_command(self.packer, interceptor_gas_cmd, frame // 2)) # ui mesg is at 100Hz but we send asap if: # - there is something to display # - there is something to stop displaying fcw_alert = hud_alert == VisualAlert.fcw steer_alert = hud_alert in [VisualAlert.steerRequired, VisualAlert.ldw] send_ui = False if ((fcw_alert or steer_alert) and not self.alert_active) or \ (not (fcw_alert or steer_alert) and self.alert_active): send_ui = True self.alert_active = not self.alert_active elif pcm_cancel_cmd: # forcing the pcm to disengage causes a bad fault sound so play a good sound instead send_ui = True # dp if not dragonconf.dpToyotaLdw: left_lane_depart = False right_lane_depart = False if (frame % 100 == 0 or send_ui): can_sends.append( create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart, enabled)) if frame % 100 == 0 and CS.CP.enableDsu: can_sends.append(create_fcw_command(self.packer, fcw_alert)) #*** static msgs *** for (addr, cars, bus, fr_step, vl) in STATIC_DSU_MSGS: if frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars: can_sends.append(make_can_msg(addr, vl, bus)) return can_sends
def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_alert, left_line, right_line, lead, left_lane_depart, right_lane_depart): # gas and brake if CS.CP.enableGasInterceptor and active: MAX_INTERCEPTOR_GAS = 0.5 # RAV4 has very sensitive gas pedal if CS.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH): PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0]) elif CS.CP.carFingerprint in (CAR.COROLLA,): PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0]) else: PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0]) # offset for creep and windbrake pedal_offset = interp(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-.4, 0.0, 0.2]) pedal_command = PEDAL_SCALE * (actuators.accel + pedal_offset) interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS) else: interceptor_gas_cmd = 0. pcm_accel_cmd = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) # steer torque new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX)) apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, CarControllerParams) self.steer_rate_limited = new_steer != apply_steer # TODO: probably can delete this. CS.pcm_acc_status uses a different signal # than CS.cruiseState.enabled. confirm they're not meaningfully different if not enabled and CS.pcm_acc_status: pcm_cancel_cmd = 1 # on entering standstill, send standstill request if CS.out.standstill and not self.last_standstill and CS.CP.carFingerprint not in NO_STOP_TIMER_CAR: self.standstill_req = True if CS.pcm_acc_status != 8: # pcm entered standstill or it's disabled self.standstill_req = False self.last_steer = apply_steer self.last_standstill = CS.out.standstill can_sends = [] if CS.CP.steerControlType == car.CarParams.SteerControlType.angle: can_sends.append(create_steer_command(self.packer, 0, 0, frame)) # On TSS2 cars, the LTA and STEER_TORQUE_SENSOR messages use relative steering angle signals that start # at 0 degrees, so we need to offset the commanded angle as well. # Also need to pulse steer_req to prevent 5 second steering lockout in some cases steer_req = active # TODO(thinh): Check and cut out steering while we are in a known fault state percentage = interp(abs(CS.out.steeringTorque), [50, 100], [100, 0]) commanded_angle = interp(percentage, [-10, 100], [CS.out.steeringAngleDeg + CS.out.steeringAngleOffsetDeg, actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg]) can_sends.append(create_lta_steer_command(self.packer, commanded_angle, steer_req, frame)) else: # Cut steering while we're in a known fault state (2s) if not active or CS.steer_state in (9, 25): apply_steer = 0 apply_steer_req = 0 else: apply_steer_req = 1 # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2; # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed # on consecutive messages can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame)) if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR: can_sends.append(create_lta_steer_command(self.packer, 0, 0, frame // 2)) # we can spam can to cancel the system even if we are using lat only control if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or pcm_cancel_cmd: lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged # Lexus IS uses a different cancellation message if pcm_cancel_cmd and CS.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC): can_sends.append(create_acc_cancel_command(self.packer)) elif CS.CP.openpilotLongitudinalControl: can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, CS.distance_btn)) self.accel = pcm_accel_cmd else: can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, CS.distance_btn)) if frame % 2 == 0 and CS.CP.enableGasInterceptor and CS.CP.openpilotLongitudinalControl: # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd. # This prevents unexpected pedal range rescaling can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, frame // 2)) self.gas = interceptor_gas_cmd # ui mesg is at 1Hz but we send asap if: # - there is something to display # - there is something to stop displaying fcw_alert = hud_alert == VisualAlert.fcw steer_alert = hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw) send_ui = False if ((fcw_alert or steer_alert) and not self.alert_active) or \ (not (fcw_alert or steer_alert) and self.alert_active): send_ui = True self.alert_active = not self.alert_active elif pcm_cancel_cmd: # forcing the pcm to disengage causes a bad fault sound so play a good sound instead send_ui = True if (frame % 100 == 0 or send_ui): can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart, enabled)) if frame % 100 == 0 and CS.CP.enableDsu: can_sends.append(create_fcw_command(self.packer, fcw_alert)) # *** static msgs *** for (addr, cars, bus, fr_step, vl) in STATIC_DSU_MSGS: if frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars: can_sends.append(make_can_msg(addr, vl, bus)) new_actuators = actuators.copy() new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX new_actuators.accel = self.accel new_actuators.gas = self.gas return new_actuators, can_sends