def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, dragonconf): """ Controls thread """ P = self.params # Send CAN commands. can_sends = [] ### STEER ### if (frame % P.STEER_STEP) == 0: final_steer = actuators.steer if enabled else 0. apply_steer = int(round(final_steer * P.STEER_MAX)) # limits due to driver torque new_steer = int(round(apply_steer)) 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 if not enabled: apply_steer = 0 # 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.dpLatCtrl, dragonconf.dpSteeringOnSignal, blinker_on or frame < self.blinker_end_frame, apply_steer) self.last_blinker_on = blinker_on can_sends.append( subarucan.create_steering_control(self.packer, apply_steer, frame, P.STEER_STEP)) self.apply_steer_last = apply_steer if self.es_distance_cnt != CS.es_distance_msg["Counter"]: can_sends.append( subarucan.create_es_distance(self.packer, CS.es_distance_msg, pcm_cancel_cmd)) self.es_distance_cnt = CS.es_distance_msg["Counter"] if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]: can_sends.append( subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, visual_alert, left_line, right_line)) self.es_lkas_cnt = CS.es_lkas_msg["Counter"] return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_lane, right_lane, left_lane_depart, right_lane_depart, dragonconf): # Steering Torque new_steer = actuators.steer * self.p.STEER_MAX apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p) self.steer_rate_limited = new_steer != apply_steer # disable if steer angle reach 90 deg, otherwise mdps fault in some models lkas_active = enabled and abs(CS.out.steeringAngle) < 90. # fix for Genesis hard fault at low speed if not self.dp_hkg_smart_mdps and CS.out.vEgo < 16.7 and self.car_fingerprint == CAR.HYUNDAI_GENESIS: lkas_active = False if not lkas_active: apply_steer = 0 self.apply_steer_last = apply_steer sys_warning, sys_state, left_lane_warning, right_lane_warning = \ process_hud_alert(enabled, self.car_fingerprint, visual_alert, left_lane, right_lane, left_lane_depart, right_lane_depart) # 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 can_sends = [] can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active, CS.lkas11, sys_warning, sys_state, enabled, left_lane, right_lane, left_lane_warning, right_lane_warning)) if pcm_cancel_cmd: can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL)) elif CS.out.cruiseState.standstill: # send resume at a max freq of 10Hz if (frame - self.last_resume_frame)*DT_CTRL > 0.1: can_sends.extend([create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL)] * 20) self.last_resume_frame = frame # 20 Hz LFA MFA message if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.IONIQ_EV_2020]: can_sends.append(create_lfa_mfa(self.packer, frame, enabled)) return can_sends
def update(self, enabled, CS, frame, actuators, dragonconf): """ Controls thread """ can_sends = [] ### STEER ### if enabled: # calculate steer and also set limits due to driver torque new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX)) apply_steer = apply_std_steer_torque_limits( new_steer, self.apply_steer_last, CS.out.steeringTorque, SteerLimitParams) self.steer_rate_limited = new_steer != apply_steer if CS.out.standstill and frame % 20 == 0: # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds # Send Resume button at 5hz if we're engaged at standstill to support full stop and go! # TODO: improve the resume trigger logic by looking at actual radar data can_sends.append( mazdacan.create_button_cmd(self.packer, CS.CP.carFingerprint, Buttons.RESUME)) else: apply_steer = 0 self.steer_rate_limited = False if CS.out.cruiseState.enabled and frame % 20 == 0: # Cancel Stock ACC if it's enabled while OP is disengaged # Send at a rate of 5hz until we sync with stock ACC state can_sends.append( mazdacan.create_button_cmd(self.packer, CS.CP.carFingerprint, Buttons.CANCEL)) # 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.apply_steer_last = apply_steer can_sends.append( mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint, frame, apply_steer, CS.cam_lkas)) return can_sends
def update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel, dragonconf): can_sends = [] steer_alert = visual_alert in [ VisualAlert.steerRequired, VisualAlert.ldw ] apply_steer = actuators.steer # 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 if pcm_cancel: #print "CANCELING!!!!" can_sends.append(spam_cancel_button(self.packer)) if (frame % 3) == 0: curvature = self.vehicle_model.calc_curvature( actuators.steeringAngleDeg * math.pi / 180., CS.out.vEgo) # The use of the toggle below is handy for trying out the various LKAS modes if TOGGLE_DEBUG: self.lkas_action += int(CS.out.genericToggle and not self.generic_toggle_last) self.lkas_action &= 0xf else: self.lkas_action = 5 # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy can_sends.append( create_steer_command(self.packer, apply_steer, enabled, CS.lkas_state, CS.out.steeringAngleDeg, curvature, self.lkas_action)) self.generic_toggle_last = CS.out.genericToggle if (frame % 100) == 0: can_sends.append( make_can_msg(973, b'\x00\x00\x00\x00\x00\x00\x00\x00', 0)) #can_sends.append(make_can_msg(984, b'\x00\x00\x00\x00\x80\x45\x60\x30', 0)) if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.out.cruiseState.available) or \ (self.steer_alert_last != steer_alert): can_sends.append( create_lkas_ui(self.packer, CS.out.cruiseState.available, enabled, steer_alert)) if (frame % 200) == 0: can_sends.append( make_can_msg(1875, b'\x80\xb0\x55\x55\x78\x90\x00\x00', 1)) if (frame % 10) == 0: can_sends.append( make_can_msg(1648, b'\x00\x00\x00\x40\x00\x00\x50\x00', 1)) can_sends.append( make_can_msg(1649, b'\x10\x10\xf1\x70\x04\x00\x00\x00', 1)) can_sends.append( make_can_msg(1664, b'\x00\x00\x03\xe8\x00\x01\xa9\xb2', 1)) can_sends.append( make_can_msg(1674, b'\x08\x00\x00\xff\x0c\xfb\x6a\x08', 1)) can_sends.append( make_can_msg(1675, b'\x00\x00\x3b\x60\x37\x00\x00\x00', 1)) can_sends.append( make_can_msg(1690, b'\x70\x00\x00\x55\x86\x1c\xe0\x00', 1)) can_sends.append( make_can_msg(1910, b'\x06\x4b\x06\x4b\x42\xd3\x11\x30', 1)) can_sends.append( make_can_msg(1911, b'\x48\x53\x37\x54\x48\x53\x37\x54', 1)) can_sends.append( make_can_msg(1912, b'\x31\x34\x47\x30\x38\x31\x43\x42', 1)) can_sends.append( make_can_msg(1913, b'\x31\x34\x47\x30\x38\x32\x43\x42', 1)) can_sends.append( make_can_msg(1969, b'\xf4\x40\x00\x00\x00\x00\x00\x00', 1)) can_sends.append( make_can_msg(1971, b'\x0b\xc0\x00\x00\x00\x00\x00\x00', 1)) static_msgs = range(1653, 1658) for addr in static_msgs: cnt = (frame % 10) + 1 can_sends.append( make_can_msg(addr, (cnt << 4).to_bytes(1, 'little') + b'\x00\x00\x00\x00\x00\x00\x00', 1)) self.enabled_last = enabled self.main_on_last = CS.out.cruiseState.available self.steer_alert_last = steer_alert return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart, dragonconf): can_sends = [] # *** steering *** if (frame % CarControllerParams.STEER_STEP) == 0: apply_steer = int( round(actuators.steer * CarControllerParams.STEER_MAX)) # limits due to driver torque new_steer = int(round(apply_steer)) apply_steer = apply_std_steer_torque_limits( new_steer, self.apply_steer_last, CS.out.steeringTorque, CarControllerParams) self.steer_rate_limited = new_steer != apply_steer if not enabled: apply_steer = 0 # 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 if CS.CP.carFingerprint in PREGLOBAL_CARS: can_sends.append( subarucan.create_preglobal_steering_control( self.packer, apply_steer, frame, CarControllerParams.STEER_STEP)) else: can_sends.append( subarucan.create_steering_control( self.packer, apply_steer, frame, CarControllerParams.STEER_STEP)) self.apply_steer_last = apply_steer # *** alerts and pcm cancel *** if CS.CP.carFingerprint in PREGLOBAL_CARS: if self.es_accel_cnt != CS.es_accel_msg["Counter"]: # 1 = main, 2 = set shallow, 3 = set deep, 4 = resume shallow, 5 = resume deep # disengage ACC when OP is disengaged if pcm_cancel_cmd: cruise_button = 1 # turn main on if off and past start-up state elif not CS.out.cruiseState.available and CS.ready: cruise_button = 1 else: cruise_button = CS.cruise_button # unstick previous mocked button press if cruise_button == 1 and self.cruise_button_prev == 1: cruise_button = 0 self.cruise_button_prev = cruise_button can_sends.append( subarucan.create_es_throttle_control( self.packer, cruise_button, CS.es_accel_msg)) self.es_accel_cnt = CS.es_accel_msg["Counter"] else: if self.es_distance_cnt != CS.es_distance_msg["Counter"]: can_sends.append( subarucan.create_es_distance(self.packer, CS.es_distance_msg, pcm_cancel_cmd)) self.es_distance_cnt = CS.es_distance_msg["Counter"] if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]: can_sends.append( subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart)) self.es_lkas_cnt = CS.es_lkas_msg["Counter"] return can_sends
def update(self, enabled, CS, frame, actuators, visual_alert, audible_alert, leftLaneVisible, rightLaneVisible, dragonconf): """ Controls thread """ P = CarControllerParams # Send CAN commands. can_sends = [] #-------------------------------------------------------------------------- # # # Prepare HCA_01 Heading Control Assist messages with steering torque. # # # #-------------------------------------------------------------------------- # The factory camera sends at 50Hz while steering and 1Hz when not. When # OP is active, Panda filters HCA_01 from the factory camera and OP emits # HCA_01 at 50Hz. Rate switching creates some confusion in Cabana and # doesn't seem to add value at this time. The rack will accept HCA_01 at # 100Hz if we want to control at finer resolution in the future. if frame % P.HCA_STEP == 0: # FAULT AVOIDANCE: HCA must not be enabled at standstill. Also stop # commanding HCA if there's a fault, so the steering rack recovers. if enabled and not (CS.out.standstill or CS.steeringFault): # FAULT AVOIDANCE: Requested HCA torque must not exceed 3.0 Nm. This # is inherently handled by scaling to STEER_MAX. The rack doesn't seem # to care about up/down rate, but we have some evidence it may do its # own rate limiting, and matching OP helps for accurate tuning. 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 # FAULT AVOIDANCE: HCA must not be enabled for >360 seconds. Sending # a single frame with HCA disabled is an effective workaround. if apply_steer == 0: # We can usually reset the timer for free, just by disabling HCA # when apply_steer is exactly zero, which happens by chance during # many steer torque direction changes. This could be expanded with # a small dead-zone to capture all zero crossings, but not seeing a # major need at this time. hcaEnabled = False self.hcaEnabledFrameCount = 0 else: self.hcaEnabledFrameCount += 1 if self.hcaEnabledFrameCount >= 118 * (100 / P.HCA_STEP): # 118s # The Kansas I-70 Crosswind Problem: if we truly do need to steer # in one direction for > 360 seconds, we have to disable HCA for a # frame while actively steering. Testing shows we can just set the # disabled flag, and keep sending non-zero torque, which keeps the # Panda torque rate limiting safety happy. Do so 3x within the 360 # second window for safety and redundancy. hcaEnabled = False self.hcaEnabledFrameCount = 0 else: hcaEnabled = True # FAULT AVOIDANCE: HCA torque must not be static for > 6 seconds. # This is to detect the sending camera being stuck or frozen. OP # can trip this on a curve if steering is saturated. Avoid this by # reducing torque 0.01 Nm for one frame. Do so 3x within the 6 # second period for safety and redundancy. if self.apply_steer_last == apply_steer: self.hcaSameTorqueCount += 1 if self.hcaSameTorqueCount > 1.9 * (100 / P.HCA_STEP): # 1.9s apply_steer -= (1, -1)[apply_steer < 0] self.hcaSameTorqueCount = 0 else: self.hcaSameTorqueCount = 0 else: # Continue sending HCA_01 messages, with the enable flags turned off. hcaEnabled = False apply_steer = 0 # 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.dpLatCtrl, dragonconf.dpSteeringOnSignal, blinker_on or frame < self.blinker_end_frame, apply_steer) self.last_blinker_on = blinker_on self.apply_steer_last = apply_steer idx = (frame / P.HCA_STEP) % 16 can_sends.append(self.create_steering_control(self.packer_pt, CANBUS.pt, apply_steer, idx, hcaEnabled)) #-------------------------------------------------------------------------- # # # Prepare LDW_02 HUD messages with lane borders, confidence levels, and # # the LKAS status LED. # # # #-------------------------------------------------------------------------- # The factory camera emits this message at 10Hz. When OP is active, Panda # filters LDW_02 from the factory camera and OP emits LDW_02 at 10Hz. if frame % P.LDW_STEP == 0: hcaEnabled = True if enabled and not CS.out.standstill else False if visual_alert == car.CarControl.HUDControl.VisualAlert.steerRequired: hud_alert = MQB_LDW_MESSAGES["laneAssistTakeOverSilent"] else: hud_alert = MQB_LDW_MESSAGES["none"] can_sends.append(self.create_hud_control(self.packer_pt, CANBUS.pt, hcaEnabled, CS.out.steeringPressed, hud_alert, leftLaneVisible, rightLaneVisible)) #-------------------------------------------------------------------------- # # # Prepare GRA_ACC_01 ACC control messages with button press events. # # # #-------------------------------------------------------------------------- # The car sends this message at 33hz. OP sends it on-demand only for # virtual button presses. # # First create any virtual button press event needed by openpilot, to sync # stock ACC with OP disengagement, or to auto-resume from stop. if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP: if not enabled and CS.out.cruiseState.enabled: # Cancel ACC if it's engaged with OP disengaged. self.graButtonStatesToSend = BUTTON_STATES.copy() self.graButtonStatesToSend["cancel"] = True elif enabled and CS.out.standstill: # Blip the Resume button if we're engaged at standstill. # FIXME: This is a naive implementation, improve with visiond or radar input. # A subset of MQBs like to "creep" too aggressively with this implementation. self.graButtonStatesToSend = BUTTON_STATES.copy() self.graButtonStatesToSend["resumeCruise"] = True # OP/Panda can see this message but can't filter it when integrated at the # R242 LKAS camera. It could do so if integrated at the J533 gateway, but # we need a generalized solution that works for either. The message is # counter-protected, so we need to time our transmissions very precisely # to achieve fast and fault-free switching between message flows accepted # at the J428 ACC radar. # # Example message flow on the bus, frequency of 33Hz (GRA_ACC_STEP): # # CAR: 0 1 2 3 4 5 6 7 8 9 A B C D E F 0 1 2 3 4 5 6 # EON: 3 4 5 6 7 8 9 A B C D E F 0 1 2 GG^ # # If OP needs to send a button press, it waits to see a GRA_ACC_01 message # counter change, and then immediately follows up with the next increment. # The OP message will be sent within about 1ms of the car's message, which # is about 2ms before the car's next message is expected. OP sends for an # arbitrary duration of 16 messages / ~0.5 sec, in lockstep with each new # message from the car. # # Because OP's counter is synced to the car, J428 immediately accepts the # OP messages as valid. Further messages from the car get discarded as # duplicates without a fault. When OP stops sending, the extra time gap # (GG) to the next valid car message is less than 1 * GRA_ACC_STEP. J428 # tolerates the gap just fine and control returns to the car immediately. if CS.graMsgBusCounter != self.graMsgBusCounterPrev: self.graMsgBusCounterPrev = CS.graMsgBusCounter if self.graButtonStatesToSend is not None: if self.graMsgSentCount == 0: self.graMsgStartFramePrev = frame idx = (CS.graMsgBusCounter + 1) % 16 can_sends.append(self.create_acc_buttons_control(self.packer_pt, self.acc_bus, self.graButtonStatesToSend, CS, idx)) self.graMsgSentCount += 1 if self.graMsgSentCount >= P.GRA_VBP_COUNT: self.graButtonStatesToSend = None self.graMsgSentCount = 0 return can_sends
def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert, dragonconf): # this seems needed to avoid steering faults and to force the sync with the EPS counter frame = CS.lkas_counter if self.prev_frame == frame: return [] # *** compute control surfaces *** # steer torque new_steer = actuators.steer * SteerLimitParams.STEER_MAX apply_steer = apply_toyota_steer_torque_limits( new_steer, self.apply_steer_last, CS.out.steeringTorqueEps, SteerLimitParams) self.steer_rate_limited = new_steer != apply_steer moving_fast = CS.out.vEgo > CS.CP.minSteerSpeed # for status message if CS.out.vEgo > (CS.CP.minSteerSpeed - 0.5): # for command high bit self.gone_fast_yet = True elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID, CAR.JEEP_CHEROKEE_2019): if CS.out.vEgo < (CS.CP.minSteerSpeed - 3.0): self.gone_fast_yet = False # < 14.5m/s stock turns off this bit, but fine down to 13.5 lkas_active = moving_fast and enabled if not lkas_active: apply_steer = 0 # 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.dpLatCtrl, dragonconf.dpSteeringOnSignal, blinker_on or frame < self.blinker_end_frame, apply_steer) self.last_blinker_on = blinker_on self.apply_steer_last = apply_steer can_sends = [] #*** control msgs *** if pcm_cancel_cmd: # TODO: would be better to start from frame_2b3 new_msg = create_wheel_buttons(self.packer, self.ccframe, cancel=True) can_sends.append(new_msg) # LKAS_HEARTBIT is forwarded by Panda so no need to send it here. # frame is 100Hz (0.01s period) if (self.ccframe % 25 == 0): # 0.25s period if (CS.lkas_car_model != -1): new_msg = create_lkas_hud(self.packer, CS.out.gearShifter, lkas_active, hud_alert, self.hud_count, CS.lkas_car_model) can_sends.append(new_msg) self.hud_count += 1 new_msg = create_lkas_command(self.packer, int(apply_steer), self.gone_fast_yet, frame) can_sends.append(new_msg) self.ccframe += 1 self.prev_frame = frame return can_sends
def update(self, enabled, CS, frame, actuators, hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, dragonconf): P = self.params # Send CAN commands. can_sends = [] # STEER if (frame % P.STEER_STEP) == 0: lkas_enabled = enabled and not CS.out.steerWarning and CS.out.vEgo > P.MIN_STEER_SPEED if lkas_enabled: new_steer = 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 # 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.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)) # GAS/BRAKE # no output if not enabled, but keep sending keepalive messages # treat pedals as one final_pedal = actuators.gas - actuators.brake if not enabled: # Stock ECU sends max regen when not enabled. apply_gas = P.MAX_ACC_REGEN apply_brake = 0 else: apply_gas = int( round(interp(final_pedal, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V))) apply_brake = int( round(interp(final_pedal, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V))) # Gas/regen and brakes - all at 25Hz if (frame % 4) == 0: idx = (frame // 4) % 4 at_full_stop = enabled and CS.out.standstill near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE) can_sends.append( gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, apply_brake, idx, near_stop, at_full_stop)) can_sends.append( gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, apply_gas, idx, enabled, at_full_stop)) # 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), # and that ADAS is alive (10hz) time_and_headlights_step = 10 tt = frame * DT_CTRL if frame % time_and_headlights_step == 0: idx = (frame // time_and_headlights_step) % 4 can_sends.append( gmcan.create_adas_time_status(CanBus.OBSTACLE, int((tt - self.start_time) * 60), idx)) can_sends.append( gmcan.create_adas_headlights_status(self.packer_obj, CanBus.OBSTACLE)) speed_and_accelerometer_step = 2 if frame % speed_and_accelerometer_step == 0: idx = (frame // speed_and_accelerometer_step) % 4 can_sends.append( gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx)) can_sends.append( gmcan.create_adas_accelerometer_speed_status( CanBus.OBSTACLE, CS.out.vEgo, idx)) 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 = CS.lkas_status == 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 == VisualAlert.steerRequired 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, visual_alert, hud_speed, left_lane, right_lane, left_lane_depart, right_lane_depart, dragonconf): # Steering Torque new_steer = int(round(actuators.steer * self.p.STEER_MAX)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p) self.steer_rate_limited = new_steer != apply_steer # disable when temp fault is active, or below LKA minimum speed lkas_active = enabled and not CS.out.steerWarning and CS.out.vEgo >= CS.CP.minSteerSpeed # fix for Genesis hard fault at low speed if not self.dp_hkg_smart_mdps and CS.out.vEgo < 16.7 and self.car_fingerprint == CAR.HYUNDAI_GENESIS: lkas_active = False if not lkas_active: apply_steer = 0 # 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.apply_steer_last = apply_steer sys_warning, sys_state, left_lane_warning, right_lane_warning = \ process_hud_alert(enabled, self.car_fingerprint, visual_alert, left_lane, right_lane, left_lane_depart, right_lane_depart) can_sends = [] # tester present - w/ no response (keeps radar disabled) if CS.CP.openpilotLongitudinalControl: if (frame % 100) == 0: can_sends.append( [0x7D0, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 0]) can_sends.append( create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active, CS.lkas11, sys_warning, sys_state, enabled, left_lane, right_lane, left_lane_warning, right_lane_warning)) if not CS.CP.openpilotLongitudinalControl: if pcm_cancel_cmd: can_sends.append( create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL)) elif CS.out.cruiseState.standstill: # send resume at a max freq of 10Hz if (frame - self.last_resume_frame) * DT_CTRL > 0.1: # send 25 messages at a time to increases the likelihood of resume being accepted can_sends.extend([ create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL) ] * 25) self.last_resume_frame = frame if frame % 2 == 0 and CS.CP.openpilotLongitudinalControl: lead_visible = False accel = actuators.accel if enabled else 0 jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7) if accel < 0: accel = interp(accel - CS.out.aEgo, [-1.0, -0.5], [2 * accel, accel]) accel = clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) stopping = (actuators.longControlState == LongCtrlState.stopping) set_speed_in_units = hud_speed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH) can_sends.extend( create_acc_commands(self.packer, enabled, accel, jerk, int(frame / 2), lead_visible, set_speed_in_units, stopping)) # 20 Hz LFA MFA message if frame % 5 == 0 and self.car_fingerprint in [ CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022 ]: can_sends.append(create_lfahda_mfc(self.packer, enabled)) # 5 Hz ACC options if frame % 20 == 0 and CS.CP.openpilotLongitudinalControl: can_sends.extend(create_acc_opt(self.packer)) # 2 Hz front radar options if frame % 50 == 0 and CS.CP.openpilotLongitudinalControl: can_sends.append(create_frt_radar_opt(self.packer)) 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, visual_alert, left_lane, right_lane, left_lane_depart, right_lane_depart, dragonconf): # Steering Torque new_steer = int(round(actuators.steer * self.p.STEER_MAX)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p) self.steer_rate_limited = new_steer != apply_steer # disable when temp fault is active, or below LKA minimum speed lkas_active = enabled and not CS.out.steerWarning and CS.out.vEgo >= CS.CP.minSteerSpeed # fix for Genesis hard fault at low speed if not self.dp_hkg_smart_mdps and CS.out.vEgo < 16.7 and self.car_fingerprint == CAR.HYUNDAI_GENESIS: lkas_active = False if not lkas_active: apply_steer = 0 # 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.apply_steer_last = apply_steer sys_warning, sys_state, left_lane_warning, right_lane_warning = \ process_hud_alert(enabled, self.car_fingerprint, visual_alert, left_lane, right_lane, left_lane_depart, right_lane_depart) can_sends = [] can_sends.append( create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active, CS.lkas11, sys_warning, sys_state, enabled, left_lane, right_lane, left_lane_warning, right_lane_warning)) if pcm_cancel_cmd: can_sends.append( create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL)) elif CS.out.cruiseState.standstill: # send resume at a max freq of 10Hz if (frame - self.last_resume_frame) * DT_CTRL > 0.1: # send 25 messages at a time to increases the likelihood of resume being accepted can_sends.extend([ create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL) ] * 25) self.last_resume_frame = frame # 20 Hz LFA MFA message if frame % 5 == 0 and self.car_fingerprint in [ CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KONA_EV, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021 ]: can_sends.append(create_lfahda_mfc(self.packer, enabled)) 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, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart, dragonconf): """ Controls thread """ can_sends = [] # **** Steering Controls ************************************************ # if frame % P.HCA_STEP == 0: # Logic to avoid HCA state 4 "refused": # * Don't steer unless HCA is in state 3 "ready" or 5 "active" # * Don't steer at standstill # * Don't send > 3.00 Newton-meters torque # * Don't send the same torque for > 6 seconds # * Don't send uninterrupted steering for > 360 seconds # One frame of HCA disabled is enough to reset the timer, without zeroing the # torque value. Do that anytime we happen to have 0 torque, or failing that, # when exceeding ~1/3 the 360 second timer. if enabled and not (CS.out.standstill or CS.out.steerError or CS.out.steerWarning): 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 if apply_steer == 0: hcaEnabled = False self.hcaEnabledFrameCount = 0 else: self.hcaEnabledFrameCount += 1 if self.hcaEnabledFrameCount >= 118 * (100 / P.HCA_STEP): # 118s hcaEnabled = False self.hcaEnabledFrameCount = 0 else: hcaEnabled = True if self.apply_steer_last == apply_steer: self.hcaSameTorqueCount += 1 if self.hcaSameTorqueCount > 1.9 * ( 100 / P.HCA_STEP): # 1.9s apply_steer -= (1, -1)[apply_steer < 0] self.hcaSameTorqueCount = 0 else: self.hcaSameTorqueCount = 0 else: hcaEnabled = False apply_steer = 0 # dp if CS.out.stopSteering: apply_steer = 0 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.apply_steer_last = apply_steer idx = (frame / P.HCA_STEP) % 16 can_sends.append( volkswagencan.create_mqb_steering_control( self.packer_pt, CANBUS.pt, apply_steer, idx, hcaEnabled)) # **** HUD Controls ***************************************************** # if frame % P.LDW_STEP == 0: if visual_alert in [VisualAlert.steerRequired, VisualAlert.ldw]: hud_alert = MQB_LDW_MESSAGES["laneAssistTakeOverSilent"] else: hud_alert = MQB_LDW_MESSAGES["none"] can_sends.append( volkswagencan.create_mqb_hud_control( self.packer_pt, CANBUS.pt, enabled, CS.out.steeringPressed, hud_alert, left_lane_visible, right_lane_visible, CS.ldw_lane_warning_left, CS.ldw_lane_warning_right, CS.ldw_side_dlc_tlc, CS.ldw_dlc, CS.ldw_tlc, CS.out.standstill, left_lane_depart, right_lane_depart)) # **** ACC Button Controls ********************************************** # # FIXME: this entire section is in desperate need of refactoring if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP: if not enabled and CS.out.cruiseState.enabled: # Cancel ACC if it's engaged with OP disengaged. self.graButtonStatesToSend = BUTTON_STATES.copy() self.graButtonStatesToSend["cancel"] = True elif enabled and CS.out.standstill: # Blip the Resume button if we're engaged at standstill. # FIXME: This is a naive implementation, improve with visiond or radar input. # A subset of MQBs like to "creep" too aggressively with this implementation. self.graButtonStatesToSend = BUTTON_STATES.copy() self.graButtonStatesToSend["resumeCruise"] = True if CS.graMsgBusCounter != self.graMsgBusCounterPrev: self.graMsgBusCounterPrev = CS.graMsgBusCounter if self.graButtonStatesToSend is not None: if self.graMsgSentCount == 0: self.graMsgStartFramePrev = frame idx = (CS.graMsgBusCounter + 1) % 16 can_sends.append( volkswagencan.create_mqb_acc_buttons_control( self.packer_pt, ext_bus, self.graButtonStatesToSend, CS, idx)) self.graMsgSentCount += 1 if self.graMsgSentCount >= P.GRA_VBP_COUNT: self.graButtonStatesToSend = None self.graMsgSentCount = 0 return can_sends
def update(self, enabled, CS, frame, actuators, cruise_cancel, hud_alert, left_line, right_line, left_lane_depart, right_lane_depart, dragonconf): """ Controls thread """ # Send CAN commands. can_sends = [] ### STEER ### acc_active = bool(CS.out.cruiseState.enabled) lkas_hud_msg = CS.lkas_hud_msg lkas_hud_info_msg = CS.lkas_hud_info_msg apply_angle = actuators.steerAngle steer_hud_alert = 1 if hud_alert == VisualAlert.steerRequired else 0 if enabled: # # windup slower if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs( self.last_angle): angle_rate_lim = interp(CS.out.vEgo, ANGLE_DELTA_BP, ANGLE_DELTA_V) else: angle_rate_lim = interp(CS.out.vEgo, ANGLE_DELTA_BP, ANGLE_DELTA_VU) apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) # Max torque from driver before EPS will give up and not apply torque if not bool(CS.out.steeringPressed): self.lkas_max_torque = LKAS_MAX_TORQUE else: # Scale max torque based on how much torque the driver is applying to the wheel self.lkas_max_torque = max( # Scale max torque down to half LKAX_MAX_TORQUE as a minimum LKAS_MAX_TORQUE * 0.5, # Start scaling torque at STEER_THRESHOLD LKAS_MAX_TORQUE - 0.6 * max(0, abs(CS.out.steeringTorque) - STEER_THRESHOLD)) else: apply_angle = CS.out.steeringAngle self.lkas_max_torque = 0 # 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_angle = common_controller_ctrl( enabled, dragonconf.dpLatCtrl, dragonconf.dpSteeringOnSignal, blinker_on or frame < self.blinker_end_frame, apply_angle) self.last_blinker_on = blinker_on self.last_angle = apply_angle if not enabled and acc_active: # send acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated cruise_cancel = 1 if self.CP.carFingerprint in [CAR.ROGUE, CAR.XTRAIL] and cruise_cancel: can_sends.append( nissancan.create_acc_cancel_cmd(self.packer, CS.cruise_throttle_msg, frame)) # TODO: Find better way to cancel! # For some reason spamming the cancel button is unreliable on the Leaf # We now cancel by making propilot think the seatbelt is unlatched, # this generates a beep and a warning message every time you disengage if self.CP.carFingerprint == CAR.LEAF and frame % 2 == 0: can_sends.append( nissancan.create_cancel_msg(self.packer, CS.cancel_msg, cruise_cancel)) can_sends.append( nissancan.create_steering_control(self.packer, self.car_fingerprint, apply_angle, frame, enabled, self.lkas_max_torque)) if frame % 2 == 0: can_sends.append( nissancan.create_lkas_hud_msg(self.packer, lkas_hud_msg, enabled, left_line, right_line, left_lane_depart, right_lane_depart)) if frame % 50 == 0: can_sends.append( nissancan.create_lkas_hud_info_msg(self.packer, lkas_hud_info_msg, steer_hud_alert)) 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_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 # *** 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 = [] # 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.isPandaBlack, 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.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 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, 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: if not dragonconf.dpAtl: 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, 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