def update(self, enabled, CS, frame, actuators): """ Controls thread """ can_sends = [] ### STEER ### if enabled: # calculate steer and also set limits due to driver torque new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX)) 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 CS.out.standstill and frame % 5 == 0: # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds # Send Resume button at 20hz 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)) 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, c, CS, frame): can_sends = [] apply_steer = 0 self.steer_rate_limited = False if c.enabled: # calculate steer and also set limits due to driver torque new_steer = int( round(c.actuators.steer * CarControllerParams.STEER_MAX)) 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 CS.out.standstill and frame % 5 == 0: # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds # Send Resume button at 20hz 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, CS.crz_btns_counter, Buttons.RESUME)) if c.cruiseControl.cancel or (CS.out.cruiseState.enabled and not c.enabled): # if brake is pressed, let us wait >20ms before trying to disable crz to avoid # a race condition with the stock system, where the second cancel from openpilot # will disable the crz 'main on' self.brake_counter = self.brake_counter + 1 if frame % 20 == 0 and not (CS.out.brakePressed and self.brake_counter < 3): # 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, CS.crz_btns_counter, Buttons.CANCEL)) else: self.brake_counter = 0 self.apply_steer_last = apply_steer # send HUD alerts if frame % 50 == 0: ldw = c.hudControl.visualAlert == VisualAlert.ldw steer_required = c.hudControl.visualAlert == VisualAlert.steerRequired can_sends.append( mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required)) # send steering command 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, 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, CC, CS): can_sends = [] apply_steer = 0 self.steer_rate_limited = False if CC.latActive: # calculate steer and also set limits due to driver torque new_steer = int(round(CC.actuators.steer * CarControllerParams.STEER_MAX)) 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 CC.cruiseControl.cancel: # If brake is pressed, let us wait >70ms before trying to disable crz to avoid # a race condition with the stock system, where the second cancel from openpilot # will disable the crz 'main on'. crz ctrl msg runs at 50hz. 70ms allows us to # read 3 messages and most likely sync state before we attempt cancel. self.brake_counter = self.brake_counter + 1 if self.frame % 10 == 0 and not (CS.out.brakePressed and self.brake_counter < 7): # Cancel Stock ACC if it's enabled while OP is disengaged # Send at a rate of 10hz until we sync with stock ACC state can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.CANCEL)) else: self.brake_counter = 0 if CC.cruiseControl.resume and self.frame % 5 == 0: # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds # Send Resume button when planner wants car to move can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME)) self.apply_steer_last = apply_steer # send HUD alerts if self.frame % 50 == 0: ldw = CC.hudControl.visualAlert == VisualAlert.ldw steer_required = CC.hudControl.visualAlert == VisualAlert.steerRequired # TODO: find a way to silence audible warnings so we can add more hud alerts steer_required = steer_required and CS.lkas_allowed_speed can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required)) # send steering command can_sends.append(mazdacan.create_steering_control(self.packer, self.CP.carFingerprint, self.frame, apply_steer, CS.cam_lkas)) new_actuators = CC.actuators.copy() new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX self.frame += 1 return new_actuators, can_sends
def update(self, sendcan, enabled, CS, frame, actuators): """ Controls thread """ P = self.params # Send CAN commands. can_sends = [] canbus = self.canbus ### STEER ### if (frame % P.STEER_STEP) == 0: final_steer = actuators.steer if enabled else 0. apply_steer = final_steer * P.STEER_MAX # limits due to driver torque driver_max_torque = P.STEER_MAX + ( P.STEER_DRIVER_ALLOWANCE + CS.steer_torque_driver * P.STEER_DRIVER_FACTOR) * P.STEER_DRIVER_MULTIPLIER driver_min_torque = -P.STEER_MAX + ( -P.STEER_DRIVER_ALLOWANCE + CS.steer_torque_driver * P.STEER_DRIVER_FACTOR) * P.STEER_DRIVER_MULTIPLIER max_steer_allowed = max(min(P.STEER_MAX, driver_max_torque), 0) min_steer_allowed = min(max(-P.STEER_MAX, driver_min_torque), 0) apply_steer = clip(apply_steer, min_steer_allowed, max_steer_allowed) # slow rate if steer torque increases in magnitude if self.apply_steer_last > 0: apply_steer = clip( apply_steer, max(self.apply_steer_last - P.STEER_DELTA_DOWN, -P.STEER_DELTA_UP), self.apply_steer_last + P.STEER_DELTA_UP) else: apply_steer = clip( apply_steer, self.apply_steer_last - P.STEER_DELTA_UP, min(self.apply_steer_last + P.STEER_DELTA_DOWN, P.STEER_DELTA_UP)) apply_steer = int(round(apply_steer)) self.apply_steer_last = apply_steer lkas_enabled = enabled and not CS.steer_not_allowed if not lkas_enabled: apply_steer = 0 if self.car_fingerprint == CAR.CX5: #counts from 0 to 15 then back to 0 ctr = (frame / P.STEER_STEP) % 16 #ctr = CS.CAM_LKAS.ctr #(frame / P.STEER_STEP) % 16 if ctr != -1 and self.last_cam_ctr != ctr: self.last_cam_ctr = ctr #line_not_visible = CS.CAM_LKAS.lnv if CS.v_ego_raw > 4: line_not_visible = 0 else: line_not_visible = 1 e1 = CS.CAM_LKAS.err1 e2 = CS.CAM_LKAS.err2 # assuming controlsd runs at 83 hz # 2000ms => 166.6 # 1000ms => 83.3 # 500ms => 41.65 tsec = 167 osec = 83 hsec = 42 qsec = 21 q3sec = 62 if CS.steer_lkas.handsoff == 1 and self.ldw == 0: self.handsoff_ctr += 1 if self.handsoff_ctr > tsec: self.ldw_ctr = osec self.handsoff_ctr = 0 self.ldw = 1 if apply_steer > 0: self.ldwr = 1 self.ldwl = 0 else: self.ldwr = 0 self.ldwl = 1 #else: # self.handsoff_ctr = 0 if self.ldw == 0 and CS.steer_lkas.block == 1 and CS.v_ego_raw > 54 and apply_steer != 0: self.ldw = 1 self.ldw_ctr = osec if apply_steer > 0: self.ldwr = 1 self.ldwl = 0 else: self.ldwr = 0 self.ldwl = 1 ldw = 0 if self.ldw_ctr > 0: self.ldw_ctr -= 1 if self.ldw_ctr == 0: self.ldw = 0 self.ldwl = 0 self.ldwr = 0 elif self.ldw_ctr < q3sec and self.ldw_ctr > qsec: ldw = 1 can_sends.append( mazdacan.create_steering_control( self.packer_pt, canbus.powertrain, CS.CP.carFingerprint, ctr, apply_steer, line_not_visible, 1, 1, e1, e2, ldw)) # send lane info msgs at 1/8 rate of steer msgs if (ctr % 8 == 0): can_sends.append( mazdacan.create_cam_lane_info( self.packer_pt, canbus.powertrain, CS.CP.carFingerprint, line_not_visible, CS.cam_laneinfo, CS.steer_lkas, self.ldwr, self.ldwl)) #can_sends.append(mazdacan.create_lkas_msg(self.packer_pt, canbus.powertrain, CS.CP.carFingerprint, CS.CAM_LKAS)) #can_sends.append(mazdacan.create_lane_track(self.packer_pt, canbus.powertrain, CS.CP.carFingerprint, CS.CAM_LT)) sendcan.send( can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())