def update(self, sendcan, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): if not self.enable_camera: return ### Steering Torque apply_steer = actuators.steer * SteerLimitParams.STEER_MAX apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, SteerLimitParams) if not enabled: apply_steer = 0 steer_req = 1 if enabled else 0 self.apply_steer_last = apply_steer can_sends = [] self.lkas11_cnt = self.cnt % 0x10 self.clu11_cnt = self.cnt % 0x10 if self.camera_disconnected: if (self.cnt % 10) == 0: can_sends.append(create_lkas12()) if (self.cnt % 50) == 0: can_sends.append(create_1191()) if (self.cnt % 7) == 0: can_sends.append(create_1156()) can_sends.append( create_lkas11(self.packer, self.car_fingerprint, apply_steer, steer_req, self.lkas11_cnt, enabled, CS.lkas11, hud_alert, keep_stock=(not self.camera_disconnected))) if pcm_cancel_cmd: can_sends.append( create_clu11(self.packer, CS.clu11, Buttons.CANCEL)) elif CS.stopped and (self.cnt - self.last_resume_cnt) > 5: self.last_resume_cnt = self.cnt can_sends.append( create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL)) ### Send messages to canbus sendcan.send( can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes()) self.cnt += 1
def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): ### Steering Torque apply_steer = actuators.steer * SteerLimitParams.STEER_MAX apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, SteerLimitParams) if not enabled: apply_steer = 0 steer_req = 1 if enabled else 0 self.apply_steer_last = apply_steer can_sends = [] self.lkas11_cnt = self.cnt % 0x10 self.clu11_cnt = self.cnt % 0x10 self.mdps12_cnt = self.cnt % 0x100 if self.camera_disconnected: if (self.cnt % 10) == 0: can_sends.append(create_lkas12()) if (self.cnt % 50) == 0: can_sends.append(create_1191()) if (self.cnt % 7) == 0: can_sends.append(create_1156()) else: can_sends.append( create_mdps12(self.packer, self.car_fingerprint, self.mdps12_cnt, CS.mdps12, CS.lkas11)) can_sends.append( create_lkas11(self.packer, self.car_fingerprint, apply_steer, steer_req, self.lkas11_cnt, enabled, CS.lkas11, hud_alert, keep_stock=(not self.camera_disconnected))) #if pcm_cancel_cmd: #can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.CANCEL)) if CS.stopped and (self.cnt - self.last_resume_cnt) > 20: if (self.cnt - self.last_resume_cnt) % 5 == 0: self.last_resume_cnt = self.cnt can_sends.append( create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL, self.clu11_cnt)) self.cnt += 1 return can_sends
def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): ### Error State Resets ### disable_steer = False can_sends = [] ### Learn Checksum ### if not self.checksum_found: # Learn Checksum from the Camera if self.checksum == "NONE": self.checksum = learn_checksum(self.packer, CS.lkas11) if self.checksum == "NONE" and self.checksum_learn_cnt < 50: self.checksum_learn_cnt += 1 return else: cloudlog.info("Discovered Checksum %s" % self.checksum) self.checksum_found = True # If MDPS is faulted from bad checksum, then cycle through all Checksums until 1 works if CS.steer_error == 1: self.camera_disconnected = True cloudlog.warning("Camera Not Detected: Brute Forcing Checksums") if self.checksum_learn_cnt > 300: self.checksum_learn_cnt = 50 if self.checksum == "NONE": cloudlog.info("Testing 6B Checksum") self.checksum = "6B" elif self.checksum == "6B": cloudlog.info("Testing 7B Checksum") self.checksum = "7B" elif self.checksum == "7B": cloudlog.info("Testing CRC8 Checksum") self.checksum = "crc8" else: self.checksum = "NONE" return else: self.checksum_learn_cnt += 1 else: cloudlog.info("Discovered Checksum %s" % self.checksum) self.checksum_found = True ### Minimum Steer Speed ### # Apply Usage of Minimum Steer Speed if CS.low_speed_alert: disable_steer = True ### Turning Indicators ### if (CS.left_blinker_on == 1 or CS.right_blinker_on == 1): self.turning_signal_timer = 100 # Disable for 1.0 Seconds after blinker turned off if self.turning_signal_timer > 0: disable_steer = True self.turning_signal_timer -= 1 ### Steering Torque ### apply_steer = actuators.steer * SteerLimitParams.STEER_MAX apply_steer = limit_steer_rate(apply_steer, self.apply_steer_last, CS.steer_torque_driver, SteerLimitParams) if not enabled or disable_steer: apply_steer = 0 steer_req = 0 else: steer_req = 1 self.apply_steer_last = apply_steer ''' ### Auto Speed Limit ### # Read Speed Limit and define if adjustment needed if (self.cnt % 50) == 0 and self.speed_enable: if not (enabled and CS.acc_active): self.speed_adjusted = False map_data = messaging.recv_one_or_none(self.map_data_sock) if map_data is not None: if bool(self.params.get("IsMetric")): self.speed_conv = CV.MS_TO_KPH else: self.speed_conv = CV.MS_TO_MPH if map_data.liveMapData.speedLimitValid: last_speed = self.map_speed v_speed = int(map_data.liveMapData.speedLimit * self.speed_offset) self.map_speed = v_speed * self.speed_conv if last_speed != self.map_speed: self.speed_adjusted = False else: self.map_speed = 0 self.speed_adjusted = True else: self.map_speed = 0 self.speed_adjusted = True # Spam buttons for Speed Adjustment if CS.acc_active and not self.speed_adjusted and self.map_speed > (8.5 * self.speed_conv) and (self.cnt % 9 == 0 or self.cnt % 9 == 1): if (CS.cruise_set_speed * self.speed_conv) > (self.map_speed * 1.005): can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.SET_DECEL, (1 if self.cnt % 9 == 1 else 0))) elif (CS.cruise_set_speed * self.speed_conv) < (self.map_speed / 1.005): can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL, (1 if self.cnt % 9 == 1 else 0))) else: self.speed_adjusted = True # Cancel Adjustment on Pedal if CS.pedal_gas: self.speed_adjusted = True ''' ### Generate CAN Messages ### self.lkas11_cnt = self.cnt % 0x10 # self.clu11_cnt = self.cnt % 0x10 self.mdps12_cnt = self.cnt % 0x100 if self.camera_disconnected: if (self.cnt % 10) == 0: can_sends.append(create_lkas12()) if (self.cnt % 50) == 0: can_sends.append(create_1191()) if (self.cnt % 7) == 0: can_sends.append(create_1156()) can_sends.append(create_lkas11(self.packer, self.car_fingerprint, apply_steer, steer_req, self.lkas11_cnt, enabled, CS.lkas11, hud_alert, (not self.camera_disconnected), self.checksum)) if not self.camera_disconnected: can_sends.append(create_mdps12(self.packer, self.car_fingerprint, self.mdps12_cnt, CS.mdps12, CS.lkas11, \ self.checksum)) # if pcm_cancel_cmd: # can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.CANCEL, 0)) if CS.stopped and (self.cnt - self.last_resume_cnt) > 20: if (self.cnt - self.last_resume_cnt) > 20: self.last_resume_cnt = self.cnt can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL, self.clu11_cnt)) self.cnt += 1 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): if CS.left_blinker_on or CS.right_blinker_on: self.turning_signal_timer = 100 # Disable for 1.0 Seconds after blinker turned off if self.turning_signal_timer: enabled = 0 ### Steering Torque apply_steer = actuators.steer * SteerLimitParams.STEER_MAX apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, SteerLimitParams) if not enabled: apply_steer = 0 steer_req = 1 if enabled else 0 self.apply_steer_last = apply_steer hud_alert, lane_visible, left_lane_warning, right_lane_warning =\ process_hud_alert(enabled, self.car_fingerprint, visual_alert, left_line, right_line,left_lane_depart, right_lane_depart) can_sends = [] self.lkas11_cnt = frame % 0x10 if self.camera_disconnected: if (frame % 10) == 0: can_sends.append(create_lkas12()) if (frame % 50) == 0: can_sends.append(create_1191()) if (frame % 7) == 0: can_sends.append(create_1156()) can_sends.append(create_lkas11(self.packer, self.car_fingerprint, apply_steer, steer_req, self.lkas11_cnt, enabled, CS.lkas11, hud_alert, lane_visible, left_lane_depart, right_lane_depart, keep_stock=(not self.camera_disconnected))) #if pcm_cancel_cmd: #self.clu11_cnt = frame % 0x10 #can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.CANCEL, self.clu11_cnt)) if CS.stopped: # run only first time when the car stops if self.last_lead_distance == 0: # get the lead distance from the Radar self.last_lead_distance = CS.lead_distance self.clu11_cnt = 0 # when lead car starts moving, create 6 RES msgs elif CS.lead_distance > self.last_lead_distance and (frame - self.last_resume_frame) > 5: can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL, self.clu11_cnt)) self.clu11_cnt += 1 # interval after 6 msgs if self.clu11_cnt > 5: self.last_resume_frame = frame self.clu11_cnt = 0 # reset lead distnce after the car starts moving elif self.last_lead_distance != 0: self.last_lead_distance = 0 if self.turning_signal_timer > 0: self.turning_signal_timer -= 1 return can_sends