def update(self, enabled, CS, frame, actuators, hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): P = self.params # Send CAN commands. can_sends = [] # STEER lkas_enabled = enabled and not CS.out.steerWarning and CS.out.vEgo > P.MIN_STEER_SPEED and CS.enable_lkas if (frame % P.STEER_STEP) == 0: if lkas_enabled: new_steer = int(round(actuators.steer * P.STEER_MAX)) apply_steer = apply_std_steer_torque_limits( new_steer, self.apply_steer_last, CS.out.steeringTorque, P) self.steer_rate_limited = new_steer != apply_steer else: apply_steer = 0 self.apply_steer_last = apply_steer idx = (frame // P.STEER_STEP) % 4 can_sends.append( gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled)) # Pedal if CS.CP.enableGasInterceptor: if (frame % 2) == 0: idx = (frame // 2) % 4 zero = 0.15625 * 2 #40/256 accel = (1 - zero) * actuators.gas + self.apply_pedal_last * zero regen_brake = zero * actuators.brake final_accel = accel - regen_brake if not enabled or not CS.adaptive_Cruise: final_accel = 0. final_accel, self.accel_steady = accel_hysteresis( final_accel, self.accel_steady) final_pedal = clip(final_accel, 0., 1.) self.apply_pedal_last = final_pedal can_sends.append( create_gas_command(self.packer_pt, final_pedal, idx)) # Send dashboard UI commands (ACC status), 25hz #if (frame % 4) == 0: # send_fcw = hud_alert == VisualAlert.fcw # can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, enabled, hud_v_cruise * CV.MS_TO_KPH, hud_show_car, send_fcw)) # Radar needs to know current speed and yaw rate (50hz) - Delete # and that ADAS is alive (10hz) #if frame % P.ADAS_KEEPALIVE_STEP == 0: # can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN) # Show green icon when LKA torque is applied, and # alarming orange icon when approaching torque limit. # If not sent again, LKA icon disappears in about 5 seconds. # Conveniently, sending camera message periodically also works as a keepalive. lka_active = lkas_enabled == 1 lka_critical = lka_active and abs(actuators.steer) > 0.9 lka_icon_status = (lka_active, lka_critical) if frame % P.CAMERA_KEEPALIVE_STEP == 0 or lka_icon_status != self.lka_icon_status_last: steer_alert = hud_alert in [ VisualAlert.steerRequired, VisualAlert.ldw ] can_sends.append( gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active, lka_critical, steer_alert)) self.lka_icon_status_last = lka_icon_status return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): P = self.params if enabled: accel = actuators.accel gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, CS.CP.carFingerprint) else: accel = 0.0 gas, brake = 0.0, 0.0 # *** apply brake hysteresis *** pre_limit_brake, self.braking, self.brake_steady = actuator_hystereses( brake, self.braking, self.brake_steady, CS.out.vEgo, CS.CP.carFingerprint) # *** no output if not enabled *** if not enabled and CS.out.cruiseState.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 = True # Never send cancel command if we never enter cruise state (no cruise if pedal) # Cancel cmd causes brakes to release at a standstill causing grinding pcm_cancel_cmd = pcm_cancel_cmd and CS.CP.pcmCruise # *** rate limit after the enable check *** self.brake_last = rate_limit(pre_limit_brake, self.brake_last, -2., DT_CTRL) # vehicle hud display, wait for one update from 10Hz 0x304 msg if hud_show_lanes: hud_lanes = 1 else: hud_lanes = 0 if enabled: if hud_show_car: hud_car = 2 else: hud_car = 1 else: hud_car = 0 fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) # **** process the car messages **** # steer torque is converted back to CAN reference (positive when steering right) apply_steer = int( interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP, P.STEER_LOOKUP_V)) lkas_active = enabled and not CS.steer_not_allowed # Send CAN commands. can_sends = [] # tester present - w/ no response (keeps radar disabled) if CS.CP.carFingerprint in HONDA_BOSCH and CS.CP.openpilotLongitudinalControl: if (frame % 10) == 0: can_sends.append( (0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 1)) # Send steering command. idx = frame % 4 can_sends.append( hondacan.create_steering_control( self.packer, apply_steer, lkas_active, CS.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl)) stopping = actuators.longControlState == LongCtrlState.stopping starting = actuators.longControlState == LongCtrlState.starting # Prevent rolling backwards accel = -4.0 if stopping else accel # wind brake from air resistance decel at high speed wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15]) # all of this is only relevant for HONDA NIDEC max_accel = interp(CS.out.vEgo, P.NIDEC_MAX_ACCEL_BP, P.NIDEC_MAX_ACCEL_V) # TODO this 1.44 is just to maintain previous behavior pcm_speed_BP = [-wind_brake, -wind_brake * (3 / 4), 0.0, 0.5] # The Honda ODYSSEY seems to have different PCM_ACCEL # msgs, is it other cars too? if CS.CP.carFingerprint in HONDA_NIDEC_ALT_PCM_ACCEL: pcm_speed_V = [ 0.0, clip(CS.out.vEgo - 3.0, 0.0, 100.0), clip(CS.out.vEgo + 0.0, 0.0, 100.0), clip(CS.out.vEgo + 5.0, 0.0, 100.0) ] pcm_accel = int((1.0) * 0xc6) else: pcm_speed_V = [ 0.0, clip(CS.out.vEgo - 2.0, 0.0, 100.0), clip(CS.out.vEgo + 2.0, 0.0, 100.0), clip(CS.out.vEgo + 5.0, 0.0, 100.0) ] pcm_accel = int(clip((accel / 1.44) / max_accel, 0.0, 1.0) * 0xc6) pcm_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V) if not CS.CP.openpilotLongitudinalControl: if (frame % 2) == 0: idx = frame // 2 can_sends.append( hondacan.create_bosch_supplemental_1( self.packer, CS.CP.carFingerprint, idx)) # If using stock ACC, spam cancel command to kill gas when OP disengages. if pcm_cancel_cmd: can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint)) elif CS.out.cruiseState.standstill: can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint)) else: # Send gas and brake commands. if (frame % 2) == 0: idx = frame // 2 ts = frame * DT_CTRL if CS.CP.carFingerprint in HONDA_BOSCH: bosch_gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V) can_sends.extend( hondacan.create_acc_commands(self.packer, enabled, accel, bosch_gas, idx, stopping, starting, CS.CP.carFingerprint)) else: apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0) apply_brake = int( clip(apply_brake * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1)) pump_on, self.last_pump_ts = brake_pump_hysteresis( apply_brake, self.apply_brake_last, self.last_pump_ts, ts) pcm_override = True can_sends.append( hondacan.create_brake_command(self.packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw_display, idx, CS.CP.carFingerprint, CS.stock_brake)) self.apply_brake_last = apply_brake if CS.CP.enableGasInterceptor: # way too aggressive at low speed without this gas_mult = interp(CS.out.vEgo, [0., 10.], [0.4, 1.0]) # 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 apply_gas = clip(gas_mult * gas, 0., 1.) can_sends.append( create_gas_command(self.packer, apply_gas, idx)) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car, hud_lanes, fcw_display, acc_alert, steer_required) # Send dashboard UI commands. if (frame % 10) == 0: idx = (frame // 10) % 4 can_sends.extend( hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.openpilotLongitudinalControl, CS.stock_hud)) return can_sends
def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera, left_line, right_line, lead, advance_time_gap = False): # *** compute control surfaces *** # gas and brake 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 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 * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX) # steer torque apply_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX)) apply_steer = apply_toyota_steer_torque_limits(apply_steer, self.last_steer, CS.steer_torque_motor, SteerLimitParams) # only cut torque when steer state is a known fault if CS.steer_state in [9, 25]: self.last_fault_frame = frame # Cut steering for 2s after fault if not enabled or (frame - self.last_fault_frame < 200): apply_steer = 0 apply_steer_req = 0 else: apply_steer_req = 1 self.steer_angle_enabled, self.ipas_reset_counter = \ ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter) #print("{0} {1} {2}".format(self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active)) # steer angle if self.steer_angle_enabled and CS.ipas_active: apply_angle = actuators.steerAngle angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V) apply_angle = clip(apply_angle, -angle_lim, angle_lim) # windup slower if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle): angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V) else: angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU) apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) else: apply_angle = CS.angle_steers 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.standstill and not self.last_standstill: 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_angle = apply_angle self.last_accel = apply_accel self.last_standstill = CS.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 if ECU.CAM in self.fake_ecus: if self.angle_control: can_sends.append(create_steer_command(self.packer, 0., 0, frame)) else: can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame)) if self.angle_control: can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled, ECU.APGS in self.fake_ecus)) elif ECU.APGS in self.fake_ecus: can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True)) # added to support OpenpilotButtons -- https://github.com/rhinodavid/OpenpilotButtons # check to see if the number of time gap lines displayed by the car matches the number # we are trying to command. if they are different, send a distance of 1 to change the displayed number of lines advance_acc_time_gap_setting = 0 now_time = sec_since_boot() if advance_time_gap and now_time - self.last_acc_advance_time > 0.05: # rate limit this to once per 0.05 seconds # was getting a soft disengage from openpilot without the rate limit advance_acc_time_gap_setting = 1 self.last_acc_advance_time = now_time # accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus): lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged if ECU.DSU in self.fake_ecus: can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, lead, advance_acc_time_gap_setting)) else: can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, advance_acc_time_gap_setting)) 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)) if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera: for addr in TARGET_IDS: can_sends.append(create_video_target(frame/10, addr)) # ui mesg is at 100Hz but we send asap if: # - there is something to display # - there is something to stop displaying alert_out = process_hud_alert(hud_alert, audible_alert) steer, fcw, sound1, sound2 = alert_out if (any(alert_out) and not self.alert_active) or \ (not any(alert_out) and self.alert_active): send_ui = True self.alert_active = not self.alert_active else: send_ui = False if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus: can_sends.append(create_ui_command(self.packer, steer, sound1, sound2, left_line, right_line)) if frame % 100 == 0 and ECU.DSU in self.fake_ecus: can_sends.append(create_fcw_command(self.packer, fcw)) #*** 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 self.car_fingerprint in cars and not (ecu == ECU.CAM and forwarding_camera): # special cases if fr_step == 5 and ecu == ECU.CAM and bus == 1: cnt = (((frame / 5) % 7) + 1) << 5 vl = chr(cnt) + vl elif addr in (0x489, 0x48a) and bus == 0: # add counter for those 2 messages (last 4 bits) cnt = ((frame/100)%0xf) + 1 if addr == 0x48a: # 0x48a has a 8 preceding the counter cnt += 1 << 7 vl += chr(cnt) can_sends.append(make_can_msg(addr, vl, bus, False)) sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera, left_line, right_line, lead, left_lane_depart, right_lane_depart): # *** compute control surfaces *** # gas and brake 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 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 * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX) # steer torque apply_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX)) apply_steer = apply_toyota_steer_torque_limits(apply_steer, self.last_steer, CS.steer_torque_motor, SteerLimitParams) # only cut torque when steer state is a known fault if CS.steer_state in [9, 25]: self.last_fault_frame = frame # Cut steering for 2s after fault if not enabled or (frame - self.last_fault_frame < 200): apply_steer = 0 apply_steer_req = 0 else: apply_steer_req = 1 self.steer_angle_enabled, self.ipas_reset_counter = \ ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter) #print("{0} {1} {2}".format(self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active)) # steer angle if self.steer_angle_enabled and CS.ipas_active: apply_angle = actuators.steerAngle angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V) apply_angle = clip(apply_angle, -angle_lim, angle_lim) # windup slower if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle): angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V) else: angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU) apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) else: apply_angle = CS.angle_steers 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.standstill and not self.last_standstill: 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_angle = apply_angle self.last_accel = apply_accel self.last_standstill = CS.standstill can_sends = [] # dragonpilot if enabled and (CS.left_blinker_on or CS.right_blinker_on) and params.get("DragonEnableSteeringOnSignal") == "1": self.turning_signal_timer = 100 if self.turning_signal_timer > 0: self.turning_signal_timer -= 1 apply_steer_req = 0 #*** 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.CAM in self.fake_ecus: if self.angle_control: can_sends.append(create_steer_command(self.packer, 0., 0, frame)) else: can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame)) if self.angle_control: can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled, ECU.APGS in self.fake_ecus)) elif ECU.APGS in self.fake_ecus: can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True)) # DragonAllowGas # if we detect gas pedal pressed, we do not want OP to apply gas or brake # gasPressed code from interface.py if CS.CP.enableGasInterceptor: # use interceptor values to disengage on pedal press gasPressed = CS.pedal_gas > 15 else: gasPressed = CS.pedal_gas > 0 if params.get("DragonAllowGas") == "1" and gasPressed: apply_accel = 0 apply_gas = 0 # accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus): lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged if ECU.DSU in self.fake_ecus: 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)) 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)) if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera: for addr in TARGET_IDS: can_sends.append(create_video_target(frame//10, addr)) # ui mesg is at 100Hz but we send asap if: # - there is something to display # - there is something to stop displaying alert_out = process_hud_alert(hud_alert, audible_alert) steer, fcw, sound1, sound2 = alert_out if (any(alert_out) and not self.alert_active) or \ (not any(alert_out) and self.alert_active): send_ui = True self.alert_active = not self.alert_active else: send_ui = False if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus: can_sends.append(create_ui_command(self.packer, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart)) if frame % 100 == 0 and ECU.DSU in self.fake_ecus and self.car_fingerprint not in TSS2_CAR: can_sends.append(create_fcw_command(self.packer, fcw)) #*** 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 self.car_fingerprint in cars and not (ecu == ECU.CAM and forwarding_camera): # special cases if fr_step == 5 and ecu == ECU.CAM and bus == 1: cnt = (((frame // 5) % 7) + 1) << 5 vl = chr(cnt) + vl elif addr in (0x489, 0x48a) and bus == 0: # add counter for those 2 messages (last 4 bits) cnt = ((frame // 100) % 0xf) + 1 if addr == 0x48a: # 0x48a has a 8 preceding the counter cnt += 1 << 7 vl += chr(cnt) can_sends.append(make_can_msg(addr, vl, bus, False)) return can_sends
def update(self, sendcan, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ hud_v_cruise, hud_show_lanes, hud_show_car, \ hud_alert, snd_beep, snd_chime): """ Controls thread """ if not self.enable_camera: return # *** apply brake hysteresis *** brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.CP.carFingerprint) # *** no output if not enabled *** 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 = True # *** rate limit after the enable check *** self.brake_last = rate_limit(brake, self.brake_last, -2., 1./100) # vehicle hud display, wait for one update from 10Hz 0x304 msg if hud_show_lanes: hud_lanes = 1 else: hud_lanes = 0 if enabled: if hud_show_car: hud_car = 2 else: hud_car = 1 else: hud_car = 0 # For lateral control-only, send chimes as a beep since we don't send 0x1fa if CS.CP.radarOffCan: snd_beep = snd_beep if snd_beep != 0 else snd_chime #print chime, alert_id, hud_alert fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required, CS.read_distance_lines) # **** process the car messages **** # *** compute control surfaces *** BRAKE_MAX = 1024/4 if CS.CP.carFingerprint in (CAR.ACURA_ILX): STEER_MAX = 0xF00 elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX): STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee) else: STEER_MAX = 0x1000 #update custom UI buttons and alerts CS.UE.update_custom_ui() if (frame % 1000 == 0): CS.cstm_btns.send_button_info() CS.UE.uiSetCarEvent(CS.cstm_btns.car_folder,CS.cstm_btns.car_name) # Get the angle from ALCA. alca_enabled = False alca_steer = 0. alca_angle = 0. turn_signal_needed = 0 # Update ALCA status and custom button every 0.1 sec. if self.ALCA.pid == None: self.ALCA.set_pid(CS) if (frame % 10 == 0): self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0) # steer torque alca_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(enabled, CS, frame, actuators) # steer torque is converted back to CAN reference (positive when steering right) apply_gas = clip(actuators.gas, 0., 1.) apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1)) apply_steer = int(clip(-alca_steer * STEER_MAX, -STEER_MAX, STEER_MAX)) if CS.cstm_btns.get_button_status("lka") == 0: apply_steer = 0 # any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5 lkas_active = enabled and not CS.steer_not_allowed # Send CAN commands. can_sends = [] # Send steering command. idx = frame % 4 can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, lkas_active, CS.CP.carFingerprint, idx)) # Send dashboard UI commands. if (frame % 10) == 0: idx = (frame/10) % 4 can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, idx)) if CS.CP.radarOffCan: # If using stock ACC, spam cancel command to kill gas when OP disengages. if pcm_cancel_cmd: can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx)) elif CS.stopped: can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx)) else: # Send gas and brake commands. if (frame % 2) == 0: idx = frame / 2 pump_on, self.last_pump_ts = brake_pump_hysteresys(apply_brake, self.apply_brake_last, self.last_pump_ts) can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, hud.chime, hud.fcw, idx)) self.apply_brake_last = apply_brake if 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, idx)) sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, sendcan, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ hud_v_cruise, hud_show_lanes, hud_show_car, \ hud_alert, snd_beep, snd_chime): """ Controls thread """ if not self.enable_camera: return # *** apply brake hysteresis *** brake, self.braking, self.brake_steady = actuator_hystereses( actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.CP.carFingerprint) # *** no output if not enabled *** 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 = True # *** rate limit after the enable check *** self.brake_last = rate_limit(brake, self.brake_last, -2., 1. / 100) # vehicle hud display, wait for one update from 10Hz 0x304 msg if hud_show_lanes and CS.lkMode and not CS.left_blinker_on and not CS.right_blinker_on: hud_lanes = 1 else: hud_lanes = 0 if enabled: if hud_show_car: hud_car = 2 else: hud_car = 1 else: hud_car = 0 # For lateral control-only, send chimes as a beep since we don't send 0x1fa if CS.CP.radarOffCan: snd_beep = snd_beep if snd_beep is not 0 else snd_chime # Do not send audible alert when steering is disabled or blinkers on #if not CS.lkMode or CS.left_blinker_on or CS.right_blinker_on: # snd_chime = 0 #print chime, alert_id, hud_alert fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required, CS.read_distance_lines, CS.lkMode, self.speed_units) # **** process the car messages **** # *** compute control surfaces *** BRAKE_MAX = 1024 / 4 if CS.CP.carFingerprint in (CAR.ACURA_ILX): STEER_MAX = 0xF00 elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX): STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee) elif CS.CP.carFingerprint in (CAR.ODYSSEY_CHN): STEER_MAX = 0x7FFF else: STEER_MAX = 0x1000 # steer torque is converted back to CAN reference (positive when steering right) apply_gas = clip(actuators.gas, 0., 1.) apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1)) apply_steer = int( clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX)) lkas_active = enabled and not CS.steer_not_allowed and CS.lkMode and not CS.left_blinker_on and not CS.right_blinker_on # add LKAS button to toggle steering # Send CAN commands. can_sends = [] # Send steering command. idx = frame % 4 can_sends.append( hondacan.create_steering_control(self.packer, apply_steer, lkas_active, CS.CP.carFingerprint, idx)) # Send dashboard UI commands. if (frame % 10) == 0: idx = (frame / 10) % 4 can_sends.extend( hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, idx)) if CS.CP.radarOffCan: # If using stock ACC, spam cancel command to kill gas when OP disengages. if pcm_cancel_cmd: can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx)) elif CS.stopped: if CS.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.INSIGHT): if CS.lead_distance > (self.prev_lead_distance + float(kegman.conf['leadDistance'])): can_sends.append( hondacan.spam_buttons_command( self.packer, CruiseButtons.RES_ACCEL, idx)) elif CS.CP.carFingerprint in (CAR.CIVIC_BOSCH): if CS.hud_lead == 1: can_sends.append( hondacan.spam_buttons_command( self.packer, CruiseButtons.RES_ACCEL, idx)) else: can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx)) else: self.prev_lead_distance = CS.lead_distance else: # Send gas and brake commands. if (frame % 2) == 0: idx = frame / 2 pump_on, self.last_pump_ts = brake_pump_hysteresys( apply_brake, self.apply_brake_last, self.last_pump_ts) can_sends.append( hondacan.create_brake_command(self.packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, hud.chime, hud.fcw, idx)) self.apply_brake_last = apply_brake if 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, idx)) sendcan.send( can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
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, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera, left_line, right_line, lead, left_lane_depart, right_lane_depart): #update custom UI buttons and alerts CS.UE.update_custom_ui() if (frame % 1000 == 0): CS.cstm_btns.send_button_info() CS.UE.uiSetCarEvent(CS.cstm_btns.car_folder, CS.cstm_btns.car_name) # *** compute control surfaces *** # gas and brake 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 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 * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX) # Get the angle from ALCA. alca_enabled = False alca_steer = 0. alca_angle = 0. turn_signal_needed = 0 # Update ALCA status and custom button every 0.1 sec. if self.ALCA.pid == None and not CS.indi_toggle: self.ALCA.set_pid(CS) if (frame % 10 == 0): self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0) # steer torque alca_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update( enabled, CS, frame, actuators) #apply_steer = int(round(alca_steer * STEER_MAX)) self.phantom.update() # steer torque if self.phantom.data["status"]: apply_steer = int(round(self.phantom.data["angle"])) if abs(CS.angle_steers) > 400: apply_steer = 0 else: apply_steer = int(round(alca_steer * SteerLimitParams.STEER_MAX)) if abs(CS.angle_steers) > 100: apply_steer = 0 if not CS.lane_departure_toggle_on: apply_steer = 0 # only cut torque when steer state is a known fault if CS.steer_state in [3, 7, 9, 11, 25]: self.last_fault_frame = frame # Cut steering for 2s after fault cutout_time = 100 if self.phantom.data["status"] else 200 if not enabled or (frame - self.last_fault_frame < cutout_time): apply_steer = 0 apply_steer_req = 0 else: apply_steer_req = 1 apply_steer = apply_toyota_steer_torque_limits(apply_steer, self.last_steer, CS.steer_torque_motor, SteerLimitParams) if apply_steer == 0 and self.last_steer == 0: apply_steer_req = 0 if not enabled and right_lane_depart and CS.v_ego > 12.5 and not CS.right_blinker_on: apply_steer = self.last_steer + 3 apply_steer = min(apply_steer, 800) #print "right" #print apply_steer apply_steer_req = 1 if not enabled and left_lane_depart and CS.v_ego > 12.5 and not CS.left_blinker_on: apply_steer = self.last_steer - 3 apply_steer = max(apply_steer, -800) #print "left" #print apply_steer apply_steer_req = 1 self.steer_angle_enabled, self.ipas_reset_counter = \ ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter) #print("{0} {1} {2}".format(self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active)) # steer angle if self.steer_angle_enabled and CS.ipas_active: apply_angle = alca_angle #apply_angle = actuators.steerAngle angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V) apply_angle = clip(apply_angle, -angle_lim, angle_lim) # windup slower if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs( self.last_angle): angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V) else: angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU) apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) else: apply_angle = CS.angle_steers 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.standstill and not self.last_standstill: # 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_angle = apply_angle self.last_accel = apply_accel self.last_standstill = CS.standstill can_sends = [] # Enable blindspot debug mode once if BLINDSPOTDEBUG: self.blindspot_poll_counter += 1 if self.blindspot_poll_counter > 1000: # 10 seconds after start if CS.left_blinker_on: self.blindspot_blink_counter_left += 1 #print "debug Left Blinker on" elif CS.right_blinker_on: self.blindspot_blink_counter_right += 1 else: self.blindspot_blink_counter_left = 0 self.blindspot_blink_counter_right = 0 #print "debug Left Blinker off" if self.blindspot_debug_enabled_left: can_sends.append( set_blindspot_debug_mode(LEFT_BLINDSPOT, False)) self.blindspot_debug_enabled_left = False #print "debug Left blindspot debug disabled" if self.blindspot_debug_enabled_right: can_sends.append( set_blindspot_debug_mode(RIGHT_BLINDSPOT, False)) self.blindspot_debug_enabled_right = False #print "debug Right blindspot debug disabled" if self.blindspot_blink_counter_left > 9 and not self.blindspot_debug_enabled_left: #check blinds can_sends.append(set_blindspot_debug_mode( LEFT_BLINDSPOT, True)) #print "debug Left blindspot debug enabled" self.blindspot_debug_enabled_left = True if self.blindspot_blink_counter_right > 5 and not self.blindspot_debug_enabled_right: #enable blindspot debug mode if CS.v_ego > 6: #polling at low speeds switches camera off can_sends.append( set_blindspot_debug_mode(RIGHT_BLINDSPOT, True)) #print "debug Right blindspot debug enabled" self.blindspot_debug_enabled_right = True if self.blindspot_debug_enabled_left: if self.blindspot_poll_counter % 20 == 0 and self.blindspot_poll_counter > 1001: # Poll blindspots at 5 Hz can_sends.append(poll_blindspot_status(LEFT_BLINDSPOT)) if self.blindspot_debug_enabled_right: if self.blindspot_poll_counter % 20 == 10 and self.blindspot_poll_counter > 1005: # Poll blindspots at 5 Hz can_sends.append(poll_blindspot_status(RIGHT_BLINDSPOT)) #*** 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.CAM in self.fake_ecus: if self.angle_control: can_sends.append( create_steer_command(self.packer, 0., 0, frame)) else: can_sends.append( create_steer_command(self.packer, apply_steer, apply_steer_req, frame)) if self.angle_control: can_sends.append( create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled, ECU.APGS in self.fake_ecus)) elif ECU.APGS in self.fake_ecus: can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True)) if CS.cstm_btns.get_button_status("tr") > 0: distance = 1 else: distance = 0 # accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or ( pcm_cancel_cmd and ECU.CAM in self.fake_ecus): lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged if ECU.DSU in self.fake_ecus: can_sends.append( create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, lead, distance)) else: can_sends.append( create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, distance)) 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)) if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera: for addr in TARGET_IDS: can_sends.append(create_video_target(frame // 10, addr)) # ui mesg is at 100Hz but we send asap if: # - there is something to display # - there is something to stop displaying alert_out = process_hud_alert(hud_alert, audible_alert) steer, fcw, sound1, sound2 = alert_out if (any(alert_out) and not self.alert_active) or \ (not any(alert_out) and self.alert_active): send_ui = True self.alert_active = not self.alert_active else: send_ui = False if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus: can_sends.append( create_ui_command(self.packer, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart)) if frame % 100 == 0 and ECU.DSU in self.fake_ecus and self.car_fingerprint not in TSSP2_CAR: can_sends.append(create_fcw_command(self.packer, fcw)) #*** 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 self.car_fingerprint in cars and not ( ecu == ECU.CAM and forwarding_camera): # special cases if fr_step == 5 and ecu == ECU.CAM and bus == 1: cnt = (((frame // 5) % 7) + 1) << 5 vl = chr(cnt) + vl elif addr in (0x489, 0x48a) and bus == 0: # add counter for those 2 messages (last 4 bits) cnt = ((frame // 100) % 0xf) + 1 if addr == 0x48a: # 0x48a has a 8 preceding the counter cnt += 1 << 7 vl += chr(cnt) can_sends.append(make_can_msg(addr, vl, bus, False)) sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
def update(self, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ hud_v_cruise, hud_show_lanes, hud_show_car, \ hud_alert, snd_beep, snd_chime): # *** apply brake hysteresis *** #brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.CP.carFingerprint) # *** no output if not enabled *** 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 = True # *** rate limit after the enable check *** #self.brake_last = rate_limit(brake, self.brake_last, -2., 1./100) # vehicle hud display, wait for one update from 10Hz 0x304 msg if hud_show_lanes or CS.lkMode: hud_lanes = 1 else: hud_lanes = 0 if enabled: if hud_show_car: hud_car = 2 else: hud_car = 1 else: hud_car = 0 # For lateral control-only, send chimes as a beep since we don't send 0x1fa if CS.CP.radarOffCan: snd_beep = snd_beep if snd_beep != 0 else snd_chime # Do not send audible alert when steering is disabled or blinkers on #if not CS.lkMode or CS.left_blinker_on or CS.right_blinker_on: # snd_chime = 0 #print("{0} {1} {2}".format(chime, alert_id, hud_alert)) fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required, CS.lkMode) # **** process the car messages **** # *** compute control surfaces *** BRAKE_MAX = 1024 // 4 if CS.CP.carFingerprint in (CAR.ACURA_ILX): STEER_MAX = 0xF00 elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX): STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee) elif CS.CP.carFingerprint in (CAR.ODYSSEY_CHN): STEER_MAX = 0x7FFF else: STEER_MAX = 0x1000 if self.BP2 is None: self.BP2 = int(kegman.conf['BP2']) if int( kegman.conf['BP2']) != 0 else STEER_MAX self.BP1 = int( kegman.conf['BP1']) if int(kegman.conf['BP1']) != 0 else 2560 print("torque breakpoints set!", self.BP1, self.BP2) # steer torque is converted back to CAN reference (positive when steering right) apply_gas = clip(actuators.gas, 0., 1.) apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1)) apply_steer = int( clip(-actuators.steer * STEER_MAX, -self.BP2, self.BP2)) apply_steer = int( interp(apply_steer, [-self.BP2, -self.BP1, self.BP1, self.BP2], [-STEER_MAX, -self.BP1, self.BP1, STEER_MAX])) lkas_active = not CS.steer_not_allowed and CS.lkMode # Send CAN commands. can_sends = [] # Send steering command. idx = frame % 4 can_sends.append( hondacan.create_steering_control(self.packer, apply_steer, lkas_active, CS.CP.carFingerprint, idx, CS.CP.isPandaBlack)) can_sends.append( hondacan.create_steering_control_x2(self.packer, CS.CP.carFingerprint, idx, CS.CP.isPandaBlack)) # Send dashboard UI commands. if (frame % 10) == 0: idx = (frame // 10) % 4 can_sends.extend( hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.isPandaBlack)) if CS.CP.radarOffCan: # If using stock ACC, spam cancel command to kill gas when OP disengages. if pcm_cancel_cmd: can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) elif CS.standstill and enabled and CS.stopped and CS.lead_distance < 200: if CS.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.INSIGHT): rough_lead_speed = self.rough_speed(CS.lead_distance) if CS.lead_distance > (self.stopped_lead_distance + 15.0) or rough_lead_speed > 0.1: #self.stopped_lead_distance = 0.0 can_sends.append( hondacan.spam_buttons_command( self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) print("spamming") print(self.stopped_lead_distance, CS.lead_distance, rough_lead_speed) elif False and CS.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.CRV_HYBRID): print(' this is a civic or crv') if CS.hud_lead == 1: can_sends.append( hondacan.spam_buttons_command( self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) else: print(' doing the regular SPAM RESUME') can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) elif CS.v_ego == 0: #print(' not quite "stopped" yet, but lead distance is %f' % CS.lead_distance) self.stopped_lead_distance = CS.lead_distance self.prev_lead_distance = CS.lead_distance else: self.stopped_lead_distance = CS.lead_distance self.prev_lead_distance = CS.lead_distance else: # Send gas and brake commands. if (frame % 2) == 0: idx = frame // 2 ts = frame * DT_CTRL pump_on, self.last_pump_ts = brake_pump_hysteresis( apply_brake, self.apply_brake_last, self.last_pump_ts, ts) can_sends.append( hondacan.create_brake_command(self.packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, hud.chime, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) self.apply_brake_last = apply_brake if 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, idx)) 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, 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 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 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 * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX) # steer torque new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX)) apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, SteerLimitParams) 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.steeringRate) > 100 or (abs(CS.out.steeringAngle) > 150 and CS.CP.carFingerprint in [CAR.RAV4H, CAR.PRIUS]): 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 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 = apply_accel 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 if Ecu.fwdCamera in self.fake_ecus: can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame)) # 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.steerAngle, 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 not dragonconf.dpAtl: 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)) 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 # dp if dragonconf.dpToyotaLdw: dragon_left_lane_depart = left_lane_depart dragon_right_lane_depart = right_lane_depart else: dragon_left_lane_depart = False dragon_right_lane_depart = False 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, dragon_left_lane_depart, dragon_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): # *** 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_speed, pcm_override, pcm_cancel_cmd, pcm_accel, hud_v_cruise, hud_show_lanes, dragonconf, hud_show_car, hud_alert): P = self.params # *** apply brake hysteresis *** brake, self.braking, self.brake_steady = actuator_hystereses( actuators.brake, self.braking, self.brake_steady, CS.out.vEgo, CS.CP.carFingerprint) # *** no output if not enabled *** if not enabled and CS.out.cruiseState.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 = True # Never send cancel command if we never enter cruise state (no cruise if pedal) # Cancel cmd causes brakes to release at a standstill causing grinding pcm_cancel_cmd = pcm_cancel_cmd and CS.CP.pcmCruise # *** rate limit after the enable check *** self.brake_last = rate_limit(brake, self.brake_last, -2., DT_CTRL) # vehicle hud display, wait for one update from 10Hz 0x304 msg if hud_show_lanes and CS.lkMode: hud_lanes = 1 else: hud_lanes = 0 if enabled: if hud_show_car: hud_car = 2 else: hud_car = 1 else: hud_car = 0 fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car, hud_lanes, fcw_display, acc_alert, steer_required, CS.lkMode) # **** process the car messages **** # steer torque is converted back to CAN reference (positive when steering right) apply_steer = int( interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP, P.STEER_LOOKUP_V)) lkas_active = enabled and not CS.steer_not_allowed and CS.lkMode # Send CAN commands. can_sends = [] # tester present - w/ no response (keeps radar disabled) if CS.CP.carFingerprint in HONDA_BOSCH and CS.CP.openpilotLongitudinalControl: if (frame % 10) == 0: can_sends.append( (0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 1)) # 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 # Send steering command. idx = frame % 4 can_sends.append( hondacan.create_steering_control( self.packer, apply_steer, lkas_active, CS.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl)) # Send dashboard UI commands. if not dragonconf.dpAtl and (frame % 10) == 0: idx = (frame // 10) % 4 can_sends.extend( hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.openpilotLongitudinalControl, CS.stock_hud)) if not CS.CP.openpilotLongitudinalControl: if (frame % 2) == 0: idx = frame // 2 can_sends.append( hondacan.create_bosch_supplemental_1( self.packer, CS.CP.carFingerprint, idx)) if dragonconf.dpAtl: pass # If using stock ACC, spam cancel command to kill gas when OP disengages. elif not dragonconf.dpAllowGas and pcm_cancel_cmd: can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint)) elif CS.out.cruiseState.standstill: if CS.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.INSIGHT): rough_lead_speed = self.rough_speed(CS.lead_distance) if CS.lead_distance > (self.stopped_lead_distance + 15.0) or rough_lead_speed > 0.1: self.stopped_lead_distance = 0.0 can_sends.append( hondacan.spam_buttons_command( self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint)) elif CS.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.CRV_HYBRID): if CS.hud_lead == 1: can_sends.append( hondacan.spam_buttons_command( self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint)) else: can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint)) else: self.stopped_lead_distance = CS.lead_distance self.prev_lead_distance = CS.lead_distance else: # Send gas and brake commands. if (frame % 2) == 0: idx = frame // 2 ts = frame * DT_CTRL if dragonconf.dpAtl: pass elif CS.CP.carFingerprint in HONDA_BOSCH: accel = actuators.gas - actuators.brake # TODO: pass in LoC.long_control_state and use that to decide starting/stoppping stopping = accel < 0 and CS.out.vEgo < 0.3 starting = accel > 0 and CS.out.vEgo < 0.3 # Prevent rolling backwards accel = -1.0 if stopping else accel apply_accel = interp(accel, P.BOSCH_ACCEL_LOOKUP_BP, P.BOSCH_ACCEL_LOOKUP_V) apply_gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V) can_sends.extend( hondacan.create_acc_commands(self.packer, enabled, apply_accel, apply_gas, idx, stopping, starting, CS.CP.carFingerprint)) else: apply_gas = clip(actuators.gas, 0., 1.) apply_brake = int( clip(self.brake_last * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1)) pump_on, self.last_pump_ts = brake_pump_hysteresis( apply_brake, self.apply_brake_last, self.last_pump_ts, ts) can_sends.append( hondacan.create_brake_command(self.packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.stock_brake)) self.apply_brake_last = apply_brake if 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, idx)) return can_sends
def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_v_cruise, hud_show_lanes, dragonconf, hud_show_car, hud_alert): P = self.params if enabled: accel = actuators.accel gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, CS.CP.carFingerprint) else: accel = 0.0 gas, brake = 0.0, 0.0 # *** apply brake hysteresis *** pre_limit_brake, self.braking, self.brake_steady = actuator_hystereses( brake, self.braking, self.brake_steady, CS.out.vEgo, CS.CP.carFingerprint) # *** rate limit after the enable check *** self.brake_last = rate_limit(pre_limit_brake, self.brake_last, -2., DT_CTRL) # vehicle hud display, wait for one update from 10Hz 0x304 msg if hud_show_lanes and CS.lkMode: hud_lanes = 1 else: hud_lanes = 0 if enabled: if hud_show_car: hud_car = 2 else: hud_car = 1 else: hud_car = 0 fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) # **** process the car messages **** # steer torque is converted back to CAN reference (positive when steering right) apply_steer = int( interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP, P.STEER_LOOKUP_V)) lkas_active = enabled and not CS.steer_not_allowed and CS.lkMode # Send CAN commands. can_sends = [] # tester present - w/ no response (keeps radar disabled) if CS.CP.carFingerprint in HONDA_BOSCH and CS.CP.openpilotLongitudinalControl: if (frame % 10) == 0: can_sends.append( (0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 1)) # 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 # Send steering command. idx = frame % 4 can_sends.append( hondacan.create_steering_control( self.packer, apply_steer, lkas_active, CS.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl)) stopping = actuators.longControlState == LongCtrlState.stopping starting = actuators.longControlState == LongCtrlState.starting # wind brake from air resistance decel at high speed wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15]) # all of this is only relevant for HONDA NIDEC max_accel = interp(CS.out.vEgo, P.NIDEC_MAX_ACCEL_BP, P.NIDEC_MAX_ACCEL_V) # TODO this 1.44 is just to maintain previous behavior pcm_speed_BP = [-wind_brake, -wind_brake * (3 / 4), 0.0, 0.5] # The Honda ODYSSEY seems to have different PCM_ACCEL # msgs, is it other cars too? if CS.CP.enableGasInterceptor: pcm_speed = 0.0 pcm_accel = int(0.0) elif CS.CP.carFingerprint in HONDA_NIDEC_ALT_PCM_ACCEL: pcm_speed_V = [ 0.0, clip(CS.out.vEgo - 3.0, 0.0, 100.0), clip(CS.out.vEgo + 0.0, 0.0, 100.0), clip(CS.out.vEgo + 5.0, 0.0, 100.0) ] pcm_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V) pcm_accel = int((1.0) * 0xc6) else: pcm_speed_V = [ 0.0, clip(CS.out.vEgo - 2.0, 0.0, 100.0), clip(CS.out.vEgo + 2.0, 0.0, 100.0), clip(CS.out.vEgo + 5.0, 0.0, 100.0) ] pcm_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V) pcm_accel = int(clip((accel / 1.44) / max_accel, 0.0, 1.0) * 0xc6) if dragonconf.dpAtl and not dragonconf.dpAtlOpLong: pass elif not CS.CP.openpilotLongitudinalControl: if (frame % 2) == 0: idx = frame // 2 can_sends.append( hondacan.create_bosch_supplemental_1( self.packer, CS.CP.carFingerprint, idx)) # If using stock ACC, spam cancel command to kill gas when OP disengages. if not dragonconf.dpAllowGas and pcm_cancel_cmd: can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint)) elif CS.out.cruiseState.standstill: if CS.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.INSIGHT): rough_lead_speed = self.rough_speed(CS.lead_distance) if CS.lead_distance > (self.stopped_lead_distance + 15.0) or rough_lead_speed > 0.1: self.stopped_lead_distance = 0.0 can_sends.append( hondacan.spam_buttons_command( self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint)) elif CS.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.CRV_HYBRID): if CS.hud_lead == 1: can_sends.append( hondacan.spam_buttons_command( self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint)) else: can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint)) else: self.stopped_lead_distance = CS.lead_distance self.prev_lead_distance = CS.lead_distance else: # Send gas and brake commands. if (frame % 2) == 0: idx = frame // 2 ts = frame * DT_CTRL if dragonconf.dpAtl and dragonconf.dpAtlOpLong and not CS.out.cruiseActualEnabled: accel = 0. gas = 0. self.brake_last = 0. if CS.CP.carFingerprint in HONDA_BOSCH: accel = clip(accel, P.BOSCH_ACCEL_MIN, P.BOSCH_ACCEL_MAX) bosch_gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V) can_sends.extend( hondacan.create_acc_commands(self.packer, enabled, active, accel, bosch_gas, idx, stopping, starting, CS.CP.carFingerprint)) else: apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0) apply_brake = int( clip(apply_brake * P.NIDEC_BRAKE_MAX, 0, P.NIDEC_BRAKE_MAX - 1)) pump_on, self.last_pump_ts = brake_pump_hysteresis( apply_brake, self.apply_brake_last, self.last_pump_ts, ts) pcm_override = True can_sends.append( hondacan.create_brake_command(self.packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw_display, idx, CS.CP.carFingerprint, CS.stock_brake)) self.apply_brake_last = apply_brake if CS.CP.enableGasInterceptor: # way too aggressive at low speed without this gas_mult = interp(CS.out.vEgo, [0., 10.], [0.4, 1.0]) # 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 # Sending non-zero gas when OP is not enabled will cause the PCM not to respond to throttle as expected # when you do enable. if enabled: apply_gas = clip( gas_mult * (gas - brake + wind_brake * 3 / 4), 0., 1.) else: apply_gas = 0.0 can_sends.append( create_gas_command(self.packer, apply_gas, idx)) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car, hud_lanes, fcw_display, acc_alert, steer_required, CS.lkMode) # Send dashboard UI commands. if dragonconf.dpAtl and not dragonconf.dpAtlOpLong: pass elif (frame % 10) == 0: idx = (frame // 10) % 4 can_sends.extend( hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.openpilotLongitudinalControl, CS.stock_hud)) 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 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 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 * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX) # steer torque apply_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX)) apply_steer = apply_toyota_steer_torque_limits(apply_steer, self.last_steer, CS.steer_torque_motor, SteerLimitParams) # only cut torque when steer state is a known fault if CS.steer_state in [9, 25]: self.last_fault_frame = frame # Cut steering for 2s after fault if not enabled or (frame - self.last_fault_frame < 200): apply_steer = 0 apply_steer_req = 0 else: apply_steer_req = 1 self.steer_angle_enabled, self.ipas_reset_counter = \ ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter) #print("{0} {1} {2}".format(self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active)) # steer angle if self.steer_angle_enabled and CS.ipas_active: apply_angle = actuators.steerAngle angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V) apply_angle = clip(apply_angle, -angle_lim, angle_lim) # windup slower if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle): angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V) else: angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU) apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) else: apply_angle = CS.angle_steers 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.standstill and not self.last_standstill: 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_angle = apply_angle self.last_accel = apply_accel self.last_standstill = CS.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 if ECU.CAM in self.fake_ecus: if self.angle_control: can_sends.append(create_steer_command(self.packer, 0., 0, frame)) else: can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame)) if self.angle_control: can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled, ECU.APGS in self.fake_ecus)) elif ECU.APGS in self.fake_ecus: can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True)) if CS.cstm_btns_tr > 0: distance = 1 else: distance = 0 # accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus): lead = lead or CS.v_ego < 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 ECU.DSU in self.fake_ecus: can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, lead, distance)) else: can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, distance)) 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 alert_out = process_hud_alert(hud_alert) steer, fcw = alert_out if (any(alert_out) and not self.alert_active) or \ (not any(alert_out) and self.alert_active): send_ui = True self.alert_active = not self.alert_active else: send_ui = False # disengage msg causes a bad fault sound so play a good sound instead if pcm_cancel_cmd: send_ui = True if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus: can_sends.append(create_ui_command(self.packer, steer, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart)) if frame % 100 == 0 and ECU.DSU in self.fake_ecus and self.car_fingerprint not in TSS2_CAR: can_sends.append(create_fcw_command(self.packer, fcw)) #*** 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 self.car_fingerprint in cars: can_sends.append(make_can_msg(addr, vl, bus, False)) return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, forwarding_camera, left_line, right_line, lead, left_lane_depart, right_lane_depart): can_sends = [] # gas and brake apply_gas = clip(actuators.gas, 0., 1.) apply_brake = clip(actuators.brake, 0., 1.) # steer torque apply_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX)) #only cut torque when steer state is a known fault if (CS.steer_state >= 1): self.last_fault_frame = frame #Cut steering for 0.5s after fault if not enabled or (frame - self.last_fault_frame < 10): 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 self.last_steer = apply_steer self.last_standstill = CS.standstill can_sends = [] if (frame % 2 == 0): # 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)) can_sends.append( create_brake_command(self.packer, apply_brake, frame // 2)) # steering at 100hz can_sends.append( create_steer_command(self.packer, apply_steer, apply_steer_req, frame)) # ui mesg is at 100Hz but we send asap if: # - there is something to display # - there is something to stop displaying alert_out = process_hud_alert(hud_alert) steer, fcw = alert_out if (any(alert_out) and not self.alert_active) or \ (not any(alert_out) and self.alert_active): send_ui = True self.alert_active = not self.alert_active else: send_ui = False # disengage msg causes a bad fault sound so play a good sound instead if pcm_cancel_cmd: send_ui = True #*** static msgs *** for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS: if frame % fr_step == 0: can_sends.append(make_toyota_can_msg(addr, vl, bus, False)) return can_sends
def update(self, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): # *** apply brake hysteresis *** brake, self.braking, self.brake_steady = actuator_hystereses( actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.CP.carFingerprint) # *** no output if not enabled *** 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 = True # *** rate limit after the enable check *** self.brake_last = rate_limit(brake, self.brake_last, -2., DT_CTRL) # vehicle hud display, wait for one update from 10Hz 0x304 msg if hud_show_lanes: hud_lanes = 1 else: hud_lanes = 0 if enabled: if hud_show_car: hud_car = 2 else: hud_car = 1 else: hud_car = 0 fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car, hud_lanes, fcw_display, acc_alert, steer_required) # **** process the car messages **** # *** compute control surfaces *** BRAKE_MAX = 1024 // 4 if CS.CP.carFingerprint in (CAR.ACURA_ILX): STEER_MAX = 0xF00 elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX): STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value elif CS.CP.carFingerprint in (CAR.ODYSSEY_CHN): STEER_MAX = 0x7FFF elif CS.CP.carFingerprint in (CAR.CIVIC) and self.eps_modified: STEER_MAX = 0x1400 else: STEER_MAX = 0x1000 # steer torque is converted back to CAN reference (positive when steering right) apply_gas = clip(actuators.gas, 0., 1.) apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1)) apply_steer = int( clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX)) if CS.CP.carFingerprint in (CAR.CIVIC) and self.eps_modified: if apply_steer > 0xA00: apply_steer = (apply_steer - 0xA00) / 2 + 0xA00 elif apply_steer < -0xA00: apply_steer = (apply_steer + 0xA00) / 2 - 0xA00 lkas_active = enabled and not CS.steer_not_allowed # Send CAN commands. can_sends = [] # Send steering command. idx = frame % 4 can_sends.append( hondacan.create_steering_control(self.packer, apply_steer, lkas_active, CS.CP.carFingerprint, idx, CS.CP.isPandaBlack)) # Send dashboard UI commands. if (frame % 10) == 0: idx = (frame // 10) % 4 can_sends.extend( hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.isPandaBlack, CS.stock_hud)) if CS.CP.radarOffCan: # If using stock ACC, spam cancel command to kill gas when OP disengages. if pcm_cancel_cmd: can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) elif CS.stopped: can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) else: # Send gas and brake commands. if (frame % 2) == 0: idx = frame // 2 ts = frame * DT_CTRL # pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts) pump_on, self.last_pump_on_state = brake_pump_hysteresis( apply_brake, self.apply_brake_last, self.last_pump_on_state, ts) if CS.CP.enableGasInterceptor: pcm_cancel_cmd = False can_sends.append( hondacan.create_brake_command(self.packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack, CS.stock_brake)) self.apply_brake_last = apply_brake if 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, idx)) 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): #if not enabled: # self.sm.update(0) #if self.sm.updated['pathPlan']: # blinker = CS.out.leftBlinker or CS.out.rightBlinker # ldw_allowed = CS.out.vEgo > 12.5 and not blinker # CAMERA_OFFSET = op_params.get('camera_offset', 0.06) # right_lane_visible = self.sm['pathPlan'].rProb > 0.5 # left_lane_visible = self.sm['pathPlan'].lProb > 0.5 # self.rightLaneDepart = bool(ldw_allowed and self.sm['pathPlan'].rPoly[3] > -(0.93 + CAMERA_OFFSET) and right_lane_visible) # self.leftLaneDepart = bool(ldw_allowed and self.sm['pathPlan'].lPoly[3] < (0.93 - CAMERA_OFFSET) and left_lane_visible) # print("blinker") # print(blinker) # print("ldw_allowed") # print(ldw_allowed) # print("CAMERA_OFFSET") # print(CAMERA_OFFSET) # print("right_lane_visible") # print(right_lane_visible) # print("left_lane_visible") # print(left_lane_visible) # print("self.rightLaneDepart") # print(self.rightLaneDepart) # print("self.leftLaneDepart") # print(self.leftLaneDepart) # *** compute control surfaces *** # gas and brake 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 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 * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX) if CS.CP.enableGasInterceptor: if CS.out.gasPressed: apply_accel = max(apply_accel, 0.06) if CS.out.brakePressed: apply_gas = 0.0 apply_accel = min(apply_accel, 0.00) else: if CS.out.gasPressed: apply_accel = max(apply_accel, 0.0) if CS.out.brakePressed and CS.out.vEgo > 1: apply_accel = min(apply_accel, 0.0) # steer torque new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX)) # only cut torque when steer state is a known fault if CS.steer_state in [9, 25]: self.last_fault_frame = frame # Cut steering for 1s after fault if (frame - self.last_fault_frame < 100) or abs( CS.out.steeringRate) > 100 or (abs(CS.out.steeringAngle) > 100 and CS.CP.carFingerprint in [CAR.RAV4H, CAR.PRIUS]): new_steer = 0 apply_steer_req = 0 else: apply_steer_req = 1 if not enabled and right_lane_depart and CS.out.vEgo > 12.5 and not CS.out.rightBlinker: new_steer = self.last_steer + 3 new_steer = min(new_steer, 800) print("right") print(new_steer) apply_steer_req = 1 if not enabled and left_lane_depart and CS.out.vEgo > 12.5 and not CS.out.leftBlinker: new_steer = self.last_steer - 3 new_steer = max(new_steer, -800) print("left") print(new_steer) apply_steer_req = 1 apply_steer = apply_toyota_steer_torque_limits( new_steer, self.last_steer, CS.out.steeringTorqueEps, SteerLimitParams) self.steer_rate_limited = new_steer != apply_steer if not enabled and abs(apply_steer) > 800 and not (right_lane_depart or left_lane_depart): apply_steer = 0 apply_steer_req = 0 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: 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 = [] #*** 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)) # 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)) 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 or Ecu.unknown 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)) # Enable blindspot debug mode once if frame > 1000 and not ( CS.CP.carFingerprint in TSS2_CAR or CS.CP.carFingerprint == CAR.CAMRY): # 10 seconds after start and not a tss2 car if BLINDSPOTALWAYSON: self.blindspot_blink_counter_left += 1 self.blindspot_blink_counter_right += 1 #print("debug blindspot alwayson!") elif CS.out.leftBlinker: self.blindspot_blink_counter_left += 1 #print("debug Left Blinker on") elif CS.out.rightBlinker: self.blindspot_blink_counter_right += 1 #print("debug Right Blinker on") else: self.blindspot_blink_counter_left = 0 self.blindspot_blink_counter_right = 0 if self.blindspot_debug_enabled_left: can_sends.append( set_blindspot_debug_mode(LEFT_BLINDSPOT, False)) #can_sends.append(make_can_msg(0x750, b'\x41\x02\x10\x01\x00\x00\x00\x00', 0)) self.blindspot_debug_enabled_left = False #print ("debug Left blindspot debug disabled") if self.blindspot_debug_enabled_right: can_sends.append( set_blindspot_debug_mode(RIGHT_BLINDSPOT, False)) #can_sends.append(make_can_msg(0x750, b'\x42\x02\x10\x01\x00\x00\x00\x00', 0)) self.blindspot_debug_enabled_right = False #print("debug Right blindspot debug disabled") if self.blindspot_blink_counter_left > 9 and not self.blindspot_debug_enabled_left: #check blinds can_sends.append(set_blindspot_debug_mode( LEFT_BLINDSPOT, True)) #can_sends.append(make_can_msg(0x750, b'\x41\x02\x10\x60\x00\x00\x00\x00', 0)) #print("debug Left blindspot debug enabled") self.blindspot_debug_enabled_left = True if self.blindspot_blink_counter_right > 5 and not self.blindspot_debug_enabled_right: #enable blindspot debug mode if CS.out.vEgo > 6: #polling at low speeds switches camera off #can_sends.append(make_can_msg(0x750, b'\x42\x02\x10\x60\x00\x00\x00\x00', 0)) can_sends.append( set_blindspot_debug_mode(RIGHT_BLINDSPOT, True)) #print("debug Right blindspot debug enabled") self.blindspot_debug_enabled_right = True if CS.out.vEgo < 6 and self.blindspot_debug_enabled_right: # if enabled and speed falls below 6m/s #can_sends.append(make_can_msg(0x750, b'\x42\x02\x10\x01\x00\x00\x00\x00', 0)) can_sends.append( set_blindspot_debug_mode(RIGHT_BLINDSPOT, False)) self.blindspot_debug_enabled_right = False #print("debug Right blindspot debug disabled") if self.blindspot_debug_enabled_left: if frame % 20 == 0 and frame > 1001: # Poll blindspots at 5 Hz can_sends.append(poll_blindspot_status(LEFT_BLINDSPOT)) #can_sends.append(make_can_msg(0x750, b'\x41\x02\x21\x69\x00\x00\x00\x00', 0)) #print("debug Left blindspot poll") if self.blindspot_debug_enabled_right: if frame % 20 == 10 and frame > 1005: # Poll blindspots at 5 Hz #can_sends.append(make_can_msg(0x750, b'\x42\x02\x21\x69\x00\x00\x00\x00', 0)) can_sends.append(poll_blindspot_status(RIGHT_BLINDSPOT)) #print("debug Right blindspot poll") 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): # dp if frame % 500 == 0: modified = get_last_modified() if self.dp_last_modified != modified: self.dragon_lane_departure_warning = False if params.get("DragonToyotaLaneDepartureWarning", encoding='utf8') == "0" else True self.dragon_toyota_sng_mod = True if params.get("DragonToyotaSnGMod", encoding='utf8') == "1" else False self.dragon_lat_ctrl, \ self.lane_change_enabled, \ self.dragon_enable_steering_on_signal = common_controller_update(self.lane_change_enabled) self.dp_last_modified = modified # *** compute control surfaces *** # gas and brake 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 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 * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX) # steer torque new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX)) apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, SteerLimitParams) self.steer_rate_limited = new_steer != apply_steer # only cut torque when steer state is a known fault if CS.steer_state in [9, 25]: self.last_fault_frame = frame # # Cut steering for 2s after fault if not enabled: # or (frame - self.last_fault_frame < 200): 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 not self.dragon_toyota_sng_mod and CS.out.standstill and not self.last_standstill: 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 # dp apply_steer_req = common_controller_ctrl(enabled, self.dragon_lat_ctrl, self.dragon_enable_steering_on_signal, CS.out.leftBlinker, CS.out.rightBlinker, apply_steer_req) 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 if Ecu.fwdCamera in self.fake_ecus: can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame)) # 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 in [CAR.LEXUS_IS, CAR.LEXUS_ISH, CAR.LEXUS_GSH]: 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)) 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 # dp if self.dragon_lane_departure_warning: dragon_left_lane_depart = left_lane_depart dragon_right_lane_depart = right_lane_depart else: dragon_left_lane_depart = False dragon_right_lane_depart = False 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, dragon_left_lane_depart, dragon_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_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): P = self.params # *** apply brake hysteresis *** brake, self.braking, self.brake_steady = actuator_hystereses( actuators.brake, self.braking, self.brake_steady, CS.out.vEgo, CS.CP.carFingerprint) # *** no output if not enabled *** if not enabled and CS.out.cruiseState.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 = True # *** rate limit after the enable check *** self.brake_last = rate_limit(brake, self.brake_last, -2., DT_CTRL) # vehicle hud display, wait for one update from 10Hz 0x304 msg if hud_show_lanes and CS.lkMode: hud_lanes = 1 else: hud_lanes = 0 if enabled: if hud_show_car: hud_car = 2 else: hud_car = 1 else: hud_car = 0 fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car, hud_lanes, fcw_display, acc_alert, steer_required, CS.read_distance_lines, CS.lkMode) # **** process the car messages **** # steer torque is converted back to CAN reference (positive when steering right) apply_gas = clip(actuators.gas, 0., 1.) # return minimum of brake_last*MAX, or MAX-1, but not less than zero apply_brake = int( clip(self.brake_last * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1)) apply_steer = int( interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP, P.STEER_LOOKUP_V)) lkas_active = enabled and not CS.steer_not_allowed and CS.lkMode #and not CS.left_blinker_on and not CS.right_blinker_on # add LKAS button to toggle steering # Send CAN commands. can_sends = [] # Send steering command. idx = frame % 4 can_sends.append( hondacan.create_steering_control(self.packer, apply_steer, lkas_active, CS.CP.carFingerprint, idx, CS.CP.isPandaBlack)) # Send dashboard UI commands. if (frame % 10) == 0: idx = (frame // 10) % 4 can_sends.extend( hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.isPandaBlack, CS.stock_hud)) if CS.CP.radarOffCan: # If using stock ACC, spam cancel command to kill gas when OP disengages. if pcm_cancel_cmd: can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) ''' elif CS.stopped: if CS.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.INSIGHT): if CS.lead_distance > (self.prev_lead_distance + float(kegman.conf['leadDistance'])): can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) elif CS.CP.carFingerprint in (CAR.CIVIC_BOSCH): if CS.hud_lead == 1: can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) else: can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) else: self.prev_lead_distance = CS.lead_distance ''' elif CS.out.cruiseState.standstill: can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) else: # Send gas and brake commands. if (frame % 2) == 0: idx = frame // 2 ts = frame * DT_CTRL pump_on, self.last_pump_on_state = brake_pump_hysteresis( apply_brake, self.apply_brake_last, self.last_pump_on_state, ts) # Do NOT send the cancel command if we are using the pedal. Sending cancel causes the car firmware to # turn the brake pump off, and we don't want that. Stock ACC does not send the cancel cmd when it is braking. if CS.CP.enableGasInterceptor: pcm_cancel_cmd = False can_sends.append( hondacan.create_brake_command(self.packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack, CS.stock_brake)) self.apply_brake_last = apply_brake if 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, idx)) return can_sends
def update(self, enabled, CS, frame, actuators, pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): P = self.params # *** apply brake hysteresis *** brake, self.braking, self.brake_steady = actuator_hystereses( actuators.brake, self.braking, self.brake_steady, CS.out.vEgo, CS.CP.carFingerprint) # *** no output if not enabled *** if not enabled and CS.out.cruiseState.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 = True # *** rate limit after the enable check *** self.brake_last = rate_limit(brake, self.brake_last, -2., DT_CTRL) # vehicle hud display, wait for one update from 10Hz 0x304 msg if hud_show_lanes: hud_lanes = 1 else: hud_lanes = 0 if enabled: if hud_show_car: hud_car = 2 else: hud_car = 1 else: hud_car = 0 fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car, hud_lanes, fcw_display, acc_alert, steer_required) # **** process the car messages **** # steer torque is converted back to CAN reference (positive when steering right) apply_steer = int( interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP, P.STEER_LOOKUP_V)) lkas_active = enabled and not CS.steer_not_allowed # Send CAN commands. can_sends = [] # Send steering command. idx = frame % 4 can_sends.append( hondacan.create_steering_control( self.packer, apply_steer, lkas_active, CS.CP.carFingerprint, idx, CS.CP.isPandaBlack, CS.CP.openpilotLongitudinalControl)) # Send dashboard UI commands. if (frame % 10) == 0: idx = (frame // 10) % 4 can_sends.extend( hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.isPandaBlack, CS.CP.openpilotLongitudinalControl, CS.stock_hud)) if not CS.CP.openpilotLongitudinalControl: if (frame % 2) == 0: idx = frame // 2 can_sends.append( hondacan.create_bosch_supplemental_1( self.packer, CS.CP.carFingerprint, idx, CS.CP.isPandaBlack)) # If using stock ACC, spam cancel command to kill gas when OP disengages. if pcm_cancel_cmd: can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) elif CS.out.cruiseState.standstill: can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) else: # Send gas and brake commands. if (frame % 2) == 0: idx = frame // 2 ts = frame * DT_CTRL if CS.CP.carFingerprint in HONDA_BOSCH: pass # TODO: implement else: apply_gas = clip(actuators.gas, 0., 1.) apply_brake = int( clip(self.brake_last * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1)) pump_on, self.last_pump_ts = brake_pump_hysteresis( apply_brake, self.apply_brake_last, self.last_pump_ts, ts) can_sends.append( hondacan.create_brake_command( self.packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack, CS.stock_brake)) self.apply_brake_last = apply_brake if 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, idx)) 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 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 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 * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX) # Get the angle from ALCA. alca_enabled = False alca_steer = 0. alca_angle = 0. turn_signal_needed = 0 # Update ALCA status and custom button every 0.1 sec. if self.ALCA.pid == None: self.ALCA.set_pid(CS) if (frame % 10 == 0): self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0) # steer torque new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX)) apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, SteerLimitParams) self.steer_rate_limited = new_steer != apply_steer # only cut torque when steer state is a known fault if CS.steer_state in [9, 25]: self.last_fault_frame = frame # Cut steering for 2s after fault if not enabled or (frame - self.last_fault_frame < 200): 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: 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 = [] #*** 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)) # 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)) 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 alert_out = process_hud_alert(hud_alert) steer, fcw = alert_out if (any(alert_out) and not self.alert_active) or \ (not any(alert_out) and self.alert_active): send_ui = True self.alert_active = not self.alert_active else: send_ui = False # disengage msg causes a bad fault sound so play a good sound instead if pcm_cancel_cmd: 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, 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)) #*** 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 self.car_fingerprint in cars: can_sends.append(make_can_msg(addr, vl, bus)) return can_sends