def test_correctness(self): # Test all cars' commands, randomize the params. for _ in xrange(1000): apply_steer = (random.randint(0, 2) % 2 == 0) frame = random.randint(1, 65536) steer_step = random.randint(1, 65536) m_old = subarucan.create_steering_control(self.subaru_cp_old, subaru_car.IMPREZA, apply_steer, frame, steer_step) m = subarucan.create_steering_control(self.subaru_cp, subaru_car.IMPREZA, apply_steer, frame, steer_step) self.assertEqual(m_old, m) m_old = subarucan.create_steering_status(self.subaru_cp_old, subaru_car.IMPREZA, apply_steer, frame, steer_step) m = subarucan.create_steering_status(self.subaru_cp, subaru_car.IMPREZA, apply_steer, frame, steer_step) self.assertEqual(m_old, m) es_distance_msg = {} pcm_cancel_cmd = (random.randint(0, 2) % 2 == 0) m_old = subarucan.create_es_distance(self.subaru_cp_old, es_distance_msg, pcm_cancel_cmd) m = subarucan.create_es_distance(self.subaru_cp, es_distance_msg, pcm_cancel_cmd) self.assertEqual(m_old, m)
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line): """ 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 apply_steer = apply_std_steer_torque_limits( apply_steer, self.apply_steer_last, CS.steer_torque_driver, P) if not enabled: apply_steer = 0. if self.car_fingerprint in (CAR.OUTBACK, CAR.LEGACY): # add noise to prevent lkas fault from constant torque value for over 1s if enabled and apply_steer == self.apply_steer_last: self.counter = +1 if self.counter == 50: apply_steer = round(int(apply_steer * 0.99)) else: self.counter = 0 can_sends.append( subarucan.create_steering_control(self.packer, CS.CP.carFingerprint, apply_steer, frame, P.STEER_STEP)) self.apply_steer_last = apply_steer if self.car_fingerprint == CAR.IMPREZA: 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"] if self.car_fingerprint in (CAR.OUTBACK, CAR.LEGACY) and pcm_cancel_cmd: can_sends.append(subarucan.create_door_control(self.packer)) return can_sends
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_line, right_line): """ Controls thread """ # dp if frame % 500 == 0: modified = get_last_modified() if self.dp_last_modified != modified: 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 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 lkas_enabled = enabled if not lkas_enabled: apply_steer = 0 # dp apply_steer = common_controller_ctrl(enabled, self.dragon_lat_ctrl, self.dragon_enable_steering_on_signal, CS.out.leftBlinker, CS.out.rightBlinker, apply_steer) 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_line, right_line): """ 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.steer_torque_driver, P) self.steer_rate_limited = new_steer != apply_steer lkas_enabled = enabled and not CS.steer_not_allowed if not lkas_enabled: apply_steer = 0 can_sends.append( subarucan.create_steering_control(self.packer, CS.CP.carFingerprint, 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, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert): """ 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 apply_steer = int(round(apply_steer)) apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, P) lkas_enabled = enabled and not CS.steer_not_allowed if not lkas_enabled: apply_steer = 0 can_sends.append(subarucan.create_steering_control(self.packer, CS.CP.carFingerprint, 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)) self.es_lkas_cnt = CS.es_lkas_msg["Counter"] sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart): can_sends = [] # *** steering *** if (frame % self.p.STEER_STEP) == 0: apply_steer = int(round(actuators.steer * self.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, self.p) self.steer_rate_limited = new_steer != apply_steer if not enabled: apply_steer = 0 if CS.CP.carFingerprint in PREGLOBAL_CARS: can_sends.append( subarucan.create_preglobal_steering_control( self.packer, apply_steer, frame, self.p.STEER_STEP)) else: can_sends.append( subarucan.create_steering_control(self.packer, apply_steer, frame, self.p.STEER_STEP)) self.apply_steer_last = apply_steer # *** alerts and pcm cancel *** if CS.CP.carFingerprint in PREGLOBAL_CARS: if self.es_distance_cnt != CS.es_distance_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_preglobal_es_distance( self.packer, cruise_button, CS.es_distance_msg)) self.es_distance_cnt = CS.es_distance_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, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart)) self.es_lkas_cnt = CS.es_lkas_msg["Counter"] new_actuators = actuators.copy() new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX return new_actuators, can_sends
def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert): #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) """ Controls thread """ P = self.params # Send CAN commands. can_sends = [] ### STEER ### if (frame % P.STEER_STEP) == 0: final_steer = alca_steer if enabled else 0. apply_steer = int(round(final_steer * P.STEER_MAX)) # limits due to driver torque apply_steer = int(round(apply_steer)) apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, P) lkas_enabled = enabled and not CS.steer_not_allowed if not lkas_enabled: apply_steer = 0 if self.car_fingerprint in (CAR.OUTBACK, CAR.LEGACY): # add noise to prevent lkas fault from constant torque value over 1s if enabled and apply_steer == self.apply_steer_last: self.counter =+ 1 if self.counter == 50: apply_steer = round(int(apply_steer * 0.99)) else: self.counter = 0 can_sends.append(subarucan.create_steering_control(self.packer, CS.CP.carFingerprint, apply_steer, frame, P.STEER_STEP)) self.apply_steer_last = apply_steer if self.car_fingerprint not in (CAR.OUTBACK, CAR.LEGACY): 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)) self.es_lkas_cnt = CS.es_lkas_msg["Counter"] if self.car_fingerprint in (CAR.OUTBACK, CAR.LEGACY) and pcm_cancel_cmd: can_sends.append(subarucan.create_door_control(self.packer)) sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
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, pcm_cancel_cmd, visual_alert, left_line, right_line): 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 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 #---------------------------------------------STOP AND GO--------------------------------------------------- if CS.CP.carFingerprint in PREGLOBAL_CARS: #PRE-GLOBAL STOP AND GO #Activate ACC Resume with throttle tap if (enabled #Cruise must be activated and CS.car_follow #Must have lead car and CS.close_distance > CarControllerParams. SNG_DISTANCE_THRESHOLD_PREGLOBAL #Distance with lead car > 3m (this is due to Preglobal ES's unreliable Close Distance signal) and CS.close_distance < 4.5 #For safety, SnG will not operate if Close Distance reads more than 4.5m (Pre-global ES's unreliability, sometimes Close Distance shows max-5m when there is a stationary object ahead) and CS.close_distance > self. prev_close_distance #Distance with lead car is increasing and CS.out.standstill #Must be standing still ): self.sng_resume_acc = True throttle_cmd = -1 #normally, just forward throttle msg from ECU if self.sng_resume_acc: #Send Maximum <THROTTLE_TAP_LIMIT> to get car out of HOLD if self.sng_throttle_tap_cnt < 5: throttle_cmd = 5 self.sng_throttle_tap_cnt += 1 else: self.sng_throttle_tap_cnt = -1 self.sng_resume_acc = False #Send throttle message if self.throttle_cnt != CS.throttle_msg["Counter"]: can_sends.append( subarucan.create_preglobal_throttle_control( self.packer, CS.throttle_msg, throttle_cmd)) self.throttle_cnt = CS.throttle_msg["Counter"] else: #GLOBAL STOP AND GO #Car can only be in HOLD state (3) if it is standing still # => if not in HOLD state car has to be moving or driver has taken action if CS.cruise_state != 3: self.sng_throttle_tap_cnt = 0 #Reset throttle tap message count when car starts moving self.sng_resume_acc = False #Cancel throttle tap when car starts moving #Reset SnG flags self.sng_resume_acc = False self.sng_send_fake_speed = False #Trigger THROTTLE TAP when in hold and close_distance increases > SNG distance threshold (with deadband) #false positives caused by pedestrians/cyclists crossing the street in front of car if (enabled and CS.cruise_state == 3 #cruise state == 3 => ACC HOLD state and CS.close_distance > CarControllerParams. SNG_DISTANCE_THRESHOLD #lead car distance is within SnG operating range and CS.close_distance < 255 and CS.close_distance > self. prev_close_distance #Distance with lead car is increasing and CS.car_follow == 1): self.sng_resume_acc = True #If standstill for 1+ seconds and CruiseState is not 3, then this car has no EPB #Exception: SUBARU ASCENT, for some reason, Ascent even on Global does not resume out of HOLD mode with throttle tap #so we prevent ASCENTs from entering HOLD in the first place if (enabled and (CS.cruise_state != 3 #cruise state == 3 => ACC HOLD state or CS.CP.carFingerprint == CAR.ASCENT ) #Except for SUBARU ASCENT and CS.out.standstill #car standstill and time.time_ns() > self.standstill_transition_timestamp + CarControllerParams.NON_EPB_STANDSTILL_THRESHOLD ): #for more than 1 second #send fake speed to ES, because we know this car has no EPB self.sng_send_fake_speed = True #default to forward wheel speed reported by ECU wheel_speed = -1 if self.sng_send_fake_speed: #only fake wheel speed if ACC engaged and car has come to a full stop for 1 second (to prevent dodgy braking) wheel_speed = CarControllerParams.NON_EPB_FAKE_SPEED #Send a throttle tap to resume ACC throttle_cmd = -1 #normally, just forward throttle msg from ECU if self.sng_resume_acc: #Send Maximum <THROTTLE_TAP_LIMIT> to get car out of HOLD if self.sng_throttle_tap_cnt < CarControllerParams.THROTTLE_TAP_LIMIT: throttle_cmd = CarControllerParams.THROTTLE_TAP_LEVEL self.sng_throttle_tap_cnt += 1 else: self.sng_throttle_tap_cnt = -1 self.sng_resume_acc = False #TODO: Send cruise throttle to get car up to speed. There is a 2-3 seconds delay after # throttle tap is sent and car start moving. EDIT: This is standard with Toyota OP's SnG #pseudo: !!!WARNING!!! Dangerous, proceed with CARE #if sng_resume_acc is True && has been 1 second since sng_resume_acc turns to True && current ES_Throttle < 2000 # send ES_Throttle = 2000 #Update prev values self.prev_close_distance = CS.close_distance self.prev_cruise_state = CS.cruise_state #Send throttle message if self.throttle_cnt != CS.throttle_msg["Counter"]: can_sends.append( subarucan.create_throttle(self.packer, CS.throttle_msg, throttle_cmd)) self.throttle_cnt = CS.throttle_msg["Counter"] #Send Brake_Pedal CAN message to fool ES if self.brake_pedal_cnt != CS.brake_pedal_msg["Counter"]: can_sends.append( subarucan.create_brake_pedal(self.packer, CS.brake_pedal_msg, wheel_speed)) self.brake_pedal_cnt = CS.brake_pedal_msg["Counter"] #record standstill time when it transitions from False to True if CS.out.standstill and not self.prev_standstill: self.standstill_transition_timestamp = time.time_ns() self.prev_standstill = CS.out.standstill #-------------------------------------------------------------------------------------------------------------- # *** 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: fake_button = 1 # turn main on if off and past start-up state elif not CS.out.cruiseState.available and CS.ready: fake_button = 1 else: fake_button = CS.button # unstick previous mocked button press if fake_button == 1 and self.fake_button_prev == 1: fake_button = 0 self.fake_button_prev = fake_button can_sends.append( subarucan.create_es_throttle_control( self.packer, fake_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)) self.es_lkas_cnt = CS.es_lkas_msg["Counter"] return can_sends
def update(self, CC, CS): actuators = CC.actuators hud_control = CC.hudControl pcm_cancel_cmd = CC.cruiseControl.cancel can_sends = [] # *** steering *** if (self.frame % self.p.STEER_STEP) == 0: apply_steer = int(round(actuators.steer * self.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, self.p) if not CC.latActive: apply_steer = 0 if self.CP.carFingerprint in PREGLOBAL_CARS: can_sends.append( subarucan.create_preglobal_steering_control( self.packer, apply_steer)) else: can_sends.append( subarucan.create_steering_control(self.packer, apply_steer)) self.apply_steer_last = apply_steer # *** alerts and pcm cancel *** if self.CP.carFingerprint in PREGLOBAL_CARS: if self.es_distance_cnt != CS.es_distance_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_preglobal_es_distance( self.packer, cruise_button, CS.es_distance_msg)) self.es_distance_cnt = CS.es_distance_msg["COUNTER"] else: if pcm_cancel_cmd and (self.frame - self.last_cancel_frame) > 0.2: bus = 1 if self.CP.carFingerprint in GLOBAL_GEN2 else 0 can_sends.append( subarucan.create_es_distance(self.packer, CS.es_distance_msg, bus, pcm_cancel_cmd)) self.last_cancel_frame = self.frame if self.es_dashstatus_cnt != CS.es_dashstatus_msg["COUNTER"]: can_sends.append( subarucan.create_es_dashstatus(self.packer, CS.es_dashstatus_msg)) self.es_dashstatus_cnt = CS.es_dashstatus_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, CC.enabled, hud_control.visualAlert, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)) self.es_lkas_cnt = CS.es_lkas_msg["COUNTER"] new_actuators = actuators.copy() new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX self.frame += 1 return new_actuators, can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line): """ Controls thread """ P = self.params # Send CAN commands. can_sends = [] ### STEER ### if (frame % P.STEER_STEP) == 0: apply_steer = int(round(actuators.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.steer_torque_driver, P) self.steer_rate_limited = new_steer != apply_steer lkas_enabled = enabled if not lkas_enabled: apply_steer = 0 self.apply_steer_last = apply_steer if not enabled: apply_steer = 0. can_sends.append( subarucan.create_steering_control(self.packer, CS.CP.carFingerprint, apply_steer, frame, P.STEER_STEP)) ### DISENGAGE ### if self.car_fingerprint == CAR.IMPREZA: if self.es_distance_cnt != CS.es_distance_msg["Counter"]: 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)) # button control if (frame % 5) == 0 and self.car_fingerprint in (CAR.OUTBACK, CAR.LEGACY, CAR.FORESTER): # 1 = main, 2 = set shallow, 3 = set deep, 4 = resume shallow, 5 = resume deep # disengage ACC when OP is disengaged if (pcm_cancel_cmd): fake_button = 1 # turn main on if off and past start-up state elif not CS.main_on and CS.ready: fake_button = 1 else: fake_button = CS.button # unstick previous mocked button press if fake_button != 0 and fake_button == self.fake_button_prev: fake_button = 0 self.fake_button_prev = fake_button can_sends.append( subarucan.create_es_throttle_control(self.packer, fake_button, CS.es_accel_msg)) ### ALERTS ### if self.car_fingerprint == CAR.IMPREZA: if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]: 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)) 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): can_sends = [] # *** steering *** if (frame % self.p.STEER_STEP) == 0: apply_steer = int(round(actuators.steer * self.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, self.p) self.steer_rate_limited = new_steer != apply_steer if not enabled: apply_steer = 0 self.torqueEPS_last = 0 self.stock_apply_last = 0 if CS.CP.carFingerprint in PREGLOBAL_CARS: can_sends.append( subarucan.create_preglobal_steering_control( self.packer, apply_steer, frame, self.p.STEER_STEP)) else: can_sends.append( subarucan.create_steering_control(self.packer, apply_steer, frame, self.p.STEER_STEP)) self.apply_steer_last = apply_steer self.torqueEPS_last = CS.out.steeringTorqueEps * self.p.STEER_EPS_MULTIPLIER self.stock_apply_last = CS.out.stockSteeringTorqueRequest * self.p.STOCK_ES_FACTOR # *** stop and go *** throttle_cmd = False speed_cmd = False if CS.CP.carFingerprint in PREGLOBAL_CARS: # Initiate the ACC resume sequence if conditions are met if (enabled # ACC active and CS.car_follow == 1 # lead car and CS.out.standstill # must be standing still and CS.close_distance > 3 # acc resume trigger threshold and CS.close_distance < 4.5 # max operating distance to filter false positives and CS.close_distance > self.prev_close_distance ): # distance with lead car is increasing self.sng_acc_resume = True # Cancel ACC if stopped, brake pressed and not stopped behind another car if enabled and CS.out.brakePressed and CS.car_follow == 0 and CS.out.standstill: pcm_cancel_cmd = True elif CS.CP.carFingerprint in GLOBAL_CARS_SNG: if CS.has_epb: # Record manual hold set while in standstill and no car in front if CS.out.standstill and self.prev_cruise_state == 1 and CS.cruise_state == 3 and CS.car_follow == 0: self.manual_hold = True # Cancel manual hold when car starts moving if not CS.out.standstill: self.manual_hold = False # Initiate the ACC resume sequence if conditions are met if (enabled # ACC active and not self.manual_hold and CS.car_follow == 1 # lead car and CS.cruise_state == 3 # ACC HOLD (only with EPB) and CS.close_distance > 150 # acc resume trigger threshold and CS.close_distance < 255 # ignore max value and CS.close_distance > self.prev_close_distance ): # distance with lead car is increasing self.sng_acc_resume = True else: if (enabled # ACC active and CS.car_follow == 1 # lead car and CS.out.standstill and frame > self.standstill_start + 50): # standstill for >0.5 second speed_cmd = True if CS.out.standstill and not self.prev_standstill: self.standstill_start = frame self.prev_standstill = CS.out.standstill self.prev_cruise_state = CS.cruise_state if self.sng_acc_resume: if self.sng_acc_resume_cnt < 5: throttle_cmd = True self.sng_acc_resume_cnt += 1 else: self.sng_acc_resume = False self.sng_acc_resume_cnt = -1 if CS.CP.carFingerprint != CAR.CROSSTREK_2020H: self.prev_close_distance = CS.close_distance # *** alerts and pcm cancel *** if CS.CP.carFingerprint in PREGLOBAL_CARS: if self.es_distance_cnt != CS.es_distance_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_preglobal_es_distance( self.packer, cruise_button, CS.es_distance_msg)) self.es_distance_cnt = CS.es_distance_msg["Counter"] if self.throttle_cnt != CS.throttle_msg["Counter"]: can_sends.append( subarucan.create_preglobal_throttle( self.packer, CS.throttle_msg, throttle_cmd)) self.throttle_cnt = CS.throttle_msg["Counter"] else: if CS.CP.carFingerprint not in [CAR.CROSSTREK_2020H, CAR.OUTBACK]: 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"] # Only cancel ACC using brake_pedal for models that do not support ES_Distance if pcm_cancel_cmd: pcm_cancel_cmd = False if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]: can_sends.append( subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart)) self.es_lkas_cnt = CS.es_lkas_msg["Counter"] if self.es_dashstatus_cnt != CS.es_dashstatus_msg["Counter"]: can_sends.append( subarucan.create_es_dashstatus(self.packer, CS.es_dashstatus_msg)) self.es_dashstatus_cnt = CS.es_dashstatus_msg["Counter"] if self.throttle_cnt != CS.throttle_msg["Counter"]: can_sends.append( subarucan.create_throttle(self.packer, CS.throttle_msg, throttle_cmd)) self.throttle_cnt = CS.throttle_msg["Counter"] if self.brake_pedal_cnt != CS.brake_pedal_msg["Counter"]: can_sends.append( subarucan.create_brake_pedal(self.packer, CS.brake_pedal_msg, speed_cmd, pcm_cancel_cmd)) self.brake_pedal_cnt = CS.brake_pedal_msg["Counter"] return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line): can_sends = [] # *** steering *** if (frame % self.params.STEER_STEP) == 0: apply_steer = int(round(actuators.steer * self.params.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, self.params) self.steer_rate_limited = new_steer != apply_steer if not enabled: apply_steer = 0 if CS.CP.carFingerprint in PREGLOBAL_CARS: can_sends.append( subarucan.create_preglobal_steering_control( self.packer, apply_steer, frame, self.params.STEER_STEP)) else: can_sends.append( subarucan.create_steering_control(self.packer, apply_steer, frame, self.params.STEER_STEP)) self.apply_steer_last = apply_steer #---------------------------------------------STOP AND GO--------------------------------------------------- if CS.CP.carFingerprint in PREGLOBAL_CARS: #PRE-GLOBAL STOP AND GO #Activate ACC Resume with throttle tap if (enabled #Cruise must be activated and CS.car_follow #Must have lead car and CS.close_distance > self.params. SNG_DISTANCE_THRESHOLD_PREGLOBAL #Distance with lead car > 3m (this is due to Preglobal ES's unreliable Close Distance signal) and CS.close_distance < 4.5 #For safety, SnG will not operate if Close Distance reads more than 4.5m (Pre-global ES's unreliability, sometimes Close Distance shows max-5m when there is a stationary object ahead) and CS.close_distance > self. prev_close_distance #Distance with lead car is increasing and CS.out.standstill #Must be standing still ): self.sng_resume_acc = True throttle_cmd = -1 #normally, just forward throttle msg from ECU if self.sng_resume_acc: #Send Maximum <THROTTLE_TAP_LIMIT> to get car out of HOLD if self.sng_throttle_tap_cnt < 5: throttle_cmd = 5 self.sng_throttle_tap_cnt += 1 else: self.sng_throttle_tap_cnt = -1 self.sng_resume_acc = False #Send throttle message if self.throttle_cnt != CS.throttle_msg["Counter"]: can_sends.append( subarucan.create_preglobal_throttle_control( self.packer, CS.throttle_msg, throttle_cmd)) self.throttle_cnt = CS.throttle_msg["Counter"] else: #GLOBAL STOP AND GO #Car can only be in HOLD state (3) if it is standing still # => if not in HOLD state car has to be moving or driver has taken action if CS.cruise_state != 3: self.sng_throttle_tap_cnt = 0 #Reset throttle tap message count when car starts moving self.sng_resume_acc = False #Cancel throttle tap when car starts moving #Trigger THROTTLE TAP when in hold and close_distance increases > SNG distance threshold (with deadband) #false positives caused by pedestrians/cyclists crossing the street in front of car self.sng_resume_acc = False if (enabled and CS.cruise_state == 3 #cruise state == 3 => ACC HOLD state and CS.close_distance > self.params. SNG_DISTANCE_THRESHOLD #lead car distance is within SnG operating range and CS.close_distance < 255 and CS.close_distance > self. prev_close_distance #Distance with lead car is increasing and CS.car_follow == 1): self.sng_resume_acc = True #Send a throttle tap to resume ACC throttle_cmd = -1 #normally, just forward throttle msg from ECU if self.sng_resume_acc: #Send Maximum <THROTTLE_TAP_LIMIT> to get car out of HOLD if self.sng_throttle_tap_cnt < self.params.THROTTLE_TAP_LIMIT: throttle_cmd = self.params.THROTTLE_TAP_LEVEL self.sng_throttle_tap_cnt += 1 else: self.sng_throttle_tap_cnt = -1 self.sng_resume_acc = False #TODO: Send cruise throttle to get car up to speed. There is a 2-3 seconds delay after # throttle tap is sent and car start moving. EDIT: This is standard with Toyota OP's SnG #pseudo: !!!WARNING!!! Dangerous, proceed with CARE #if sng_resume_acc is True && has been 1 second since sng_resume_acc turns to True && current ES_Throttle < 2000 # send ES_Throttle = 2000 #Update prev values self.prev_close_distance = CS.close_distance self.prev_cruise_state = CS.cruise_state #Send throttle message if self.throttle_cnt != CS.throttle_msg["Counter"]: can_sends.append( subarucan.create_throttle(self.packer, CS.throttle_msg, throttle_cmd)) self.throttle_cnt = CS.throttle_msg["Counter"] #-------------------------------------------------------------------------------------------------------------- # *** 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: fake_button = 1 # turn main on if off and past start-up state elif not CS.out.cruiseState.available and CS.ready: fake_button = 1 else: fake_button = CS.button # unstick previous mocked button press if fake_button == 1 and self.fake_button_prev == 1: fake_button = 0 self.fake_button_prev = fake_button can_sends.append( subarucan.create_es_throttle_control( self.packer, fake_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)) self.es_lkas_cnt = CS.es_lkas_msg["Counter"] return can_sends