diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 11c2d367cb..e882ef4b64 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -10,12 +10,13 @@ from selfdrive.car.gm.values import DBC, CanBus, CarControllerParams VisualAlert = car.CarControl.HUDControl.VisualAlert -class CarController(): +class CarController: def __init__(self, dbc_name, CP, VM): self.start_time = 0. self.apply_steer_last = 0 self.apply_gas = 0 self.apply_brake = 0 + self.frame = 0 self.lka_steering_cmd_counter_last = -1 self.lka_icon_status_last = (False, False) @@ -27,9 +28,13 @@ class CarController(): self.packer_obj = CANPacker(DBC[CP.carFingerprint]['radar']) self.packer_ch = CANPacker(DBC[CP.carFingerprint]['chassis']) - def update(self, c, CS, frame, actuators, hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): - - P = self.params + def update(self, CC, CS): + actuators = CC.actuators + hud_control = CC.hudControl + hud_alert = hud_control.visualAlert + hud_v_cruise = hud_control.setSpeed + if hud_v_cruise > 70: + hud_v_cruise = 0 # Send CAN commands. can_sends = [] @@ -39,11 +44,11 @@ class CarController(): # next Panda loopback confirmation in the current CS frame. if CS.lka_steering_cmd_counter != self.lka_steering_cmd_counter_last: self.lka_steering_cmd_counter_last = CS.lka_steering_cmd_counter - elif (frame % P.STEER_STEP) == 0: - lkas_enabled = c.latActive and CS.out.vEgo > P.MIN_STEER_SPEED + elif (self.frame % self.params.STEER_STEP) == 0: + lkas_enabled = CC.latActive and CS.out.vEgo > self.params.MIN_STEER_SPEED if lkas_enabled: - new_steer = int(round(actuators.steer * P.STEER_MAX)) - apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P) + new_steer = int(round(actuators.steer * self.params.STEER_MAX)) + 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 else: apply_steer = 0 @@ -56,44 +61,45 @@ class CarController(): can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled)) # Gas/regen and brakes - all at 25Hz - if (frame % 4) == 0: - if not c.longActive: + if (self.frame % 4) == 0: + if not CC.longActive: # Stock ECU sends max regen when not enabled. - self.apply_gas = P.MAX_ACC_REGEN + self.apply_gas = self.params.MAX_ACC_REGEN self.apply_brake = 0 else: - self.apply_gas = int(round(interp(actuators.accel, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V))) - self.apply_brake = int(round(interp(actuators.accel, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V))) + self.apply_gas = int(round(interp(actuators.accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) + self.apply_brake = int(round(interp(actuators.accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) - idx = (frame // 4) % 4 + idx = (self.frame // 4) % 4 - at_full_stop = c.longActive and CS.out.standstill - near_stop = c.longActive and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE) + at_full_stop = CC.longActive and CS.out.standstill + near_stop = CC.longActive and (CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE) can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, self.apply_brake, idx, near_stop, at_full_stop)) - can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, c.longActive, at_full_stop)) + can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, CC.longActive, at_full_stop)) # Send dashboard UI commands (ACC status), 25hz - if (frame % 4) == 0: + if (self.frame % 4) == 0: send_fcw = hud_alert == VisualAlert.fcw - can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, c.longActive, hud_v_cruise * CV.MS_TO_KPH, hud_show_car, send_fcw)) + can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.longActive, + hud_v_cruise * CV.MS_TO_KPH, hud_control.leadVisible, 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 + tt = self.frame * DT_CTRL - if frame % time_and_headlights_step == 0: - idx = (frame // time_and_headlights_step) % 4 + if self.frame % time_and_headlights_step == 0: + idx = (self.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 + if self.frame % speed_and_accelerometer_step == 0: + idx = (self.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: + if self.frame % self.params.ADAS_KEEPALIVE_STEP == 0: can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN) # Show green icon when LKA torque is applied, and @@ -103,14 +109,15 @@ class CarController(): 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: + if self.frame % self.params.CAMERA_KEEPALIVE_STEP == 0 or lka_icon_status != self.lka_icon_status_last: steer_alert = hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw) can_sends.append(gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active, lka_critical, steer_alert)) self.lka_icon_status_last = lka_icon_status new_actuators = actuators.copy() - new_actuators.steer = self.apply_steer_last / P.STEER_MAX + new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX new_actuators.gas = self.apply_gas new_actuators.brake = self.apply_brake + self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 7a0d20b4e4..5787dc5ae8 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -216,14 +216,5 @@ class CarInterface(CarInterfaceBase): return self.CS.out def apply(self, c): - hud_control = c.hudControl - hud_v_cruise = hud_control.setSpeed - if hud_v_cruise > 70: - hud_v_cruise = 0 - - ret = self.CC.update(c, self.CS, self.frame, c.actuators, - hud_v_cruise, hud_control.lanesVisible, - hud_control.leadVisible, hud_control.visualAlert) - - self.frame += 1 + ret = self.CC.update(c, self.CS) return ret diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 2fd7fb33ac..9abdf8d03a 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -88,17 +88,18 @@ class CarController: self.last_resume_frame = self.frame if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl: - lead_visible = False - accel = actuators.accel if CC.longActive else 0 - - jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7) + accel = actuators.accel + jerk = 0 - if accel < 0: - accel = interp(accel - CS.out.aEgo, [-1.0, -0.5], [2 * accel, accel]) + if CC.longActive: + 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) + lead_visible = False + stopping = actuators.longControlState == LongCtrlState.stopping set_speed_in_units = hud_control.setSpeed * (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, CC.enabled, accel, jerk, int(self.frame / 2), lead_visible, set_speed_in_units, stopping, CS.out.gasPressed)) diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index ba2e3277f0..d03eb135fc 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -96,9 +96,9 @@ def create_acc_commands(packer, enabled, accel, jerk, idx, lead_visible, set_spe scc12_values = { "ACCMode": 2 if enabled and gas_pressed else 1 if enabled else 0, - "StopReq": 1 if enabled and stopping else 0, - "aReqRaw": accel if enabled else 0, - "aReqValue": accel if enabled else 0, # stock ramps up and down respecting jerk limit until it reaches aReqRaw + "StopReq": 1 if stopping else 0, + "aReqRaw": accel, + "aReqValue": accel, # stock ramps up and down respecting jerk limit until it reaches aReqRaw "CR_VSM_Alive": idx % 0xF, } scc12_dat = packer.make_can_msg("SCC12", 0, scc12_values)[2] @@ -109,8 +109,8 @@ def create_acc_commands(packer, enabled, accel, jerk, idx, lead_visible, set_spe scc14_values = { "ComfortBandUpper": 0.0, # stock usually is 0 but sometimes uses higher values "ComfortBandLower": 0.0, # stock usually is 0 but sometimes uses higher values - "JerkUpperLimit": max(jerk, 1.0) if (enabled and not stopping) else 0, # stock usually is 1.0 but sometimes uses higher values - "JerkLowerLimit": max(-jerk, 1.0) if enabled else 0, # stock usually is 0.5 but sometimes uses higher values + "JerkUpperLimit": max(jerk, 1.0) if not stopping else 0, # stock usually is 1.0 but sometimes uses higher values + "JerkLowerLimit": max(-jerk, 1.0), # stock usually is 0.5 but sometimes uses higher values "ACCMode": 2 if enabled and gas_pressed else 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage "ObjGap": 2 if lead_visible else 0, # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead } diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index d5a5bb8629..2c1f55e7de 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -3,41 +3,46 @@ from opendbc.can.packer import CANPacker from selfdrive.car.tesla.teslacan import TeslaCAN from selfdrive.car.tesla.values import DBC, CANBUS, CarControllerParams -class CarController(): + +class CarController: def __init__(self, dbc_name, CP, VM): self.CP = CP + self.frame = 0 self.last_angle = 0 self.long_control_counter = 0 self.packer = CANPacker(dbc_name) self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt']) self.tesla_can = TeslaCAN(self.packer, self.pt_packer) - def update(self, c, CS, frame, actuators, cruise_cancel): + def update(self, CC, CS): + actuators = CC.actuators + pcm_cancel_cmd = CC.cruiseControl.cancel + can_sends = [] # Temp disable steering on a hands_on_fault, and allow for user override - hands_on_fault = (CS.steer_warning == "EAC_ERROR_HANDS_ON" and CS.hands_on_level >= 3) - lkas_enabled = c.latActive and (not hands_on_fault) + hands_on_fault = CS.steer_warning == "EAC_ERROR_HANDS_ON" and CS.hands_on_level >= 3 + lkas_enabled = CC.latActive and not hands_on_fault if lkas_enabled: apply_angle = actuators.steeringAngleDeg # Angular rate limit based on speed - steer_up = (self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle)) + steer_up = self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle) rate_limit = CarControllerParams.RATE_LIMIT_UP if steer_up else CarControllerParams.RATE_LIMIT_DOWN max_angle_diff = interp(CS.out.vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points) - apply_angle = clip(apply_angle, (self.last_angle - max_angle_diff), (self.last_angle + max_angle_diff)) + apply_angle = clip(apply_angle, self.last_angle - max_angle_diff, self.last_angle + max_angle_diff) # To not fault the EPS - apply_angle = clip(apply_angle, (CS.out.steeringAngleDeg - 20), (CS.out.steeringAngleDeg + 20)) + apply_angle = clip(apply_angle, CS.out.steeringAngleDeg - 20, CS.out.steeringAngleDeg + 20) else: apply_angle = CS.out.steeringAngleDeg self.last_angle = apply_angle - can_sends.append(self.tesla_can.create_steering_control(apply_angle, lkas_enabled, frame)) + can_sends.append(self.tesla_can.create_steering_control(apply_angle, lkas_enabled, self.frame)) # Longitudinal control (40Hz) - if self.CP.openpilotLongitudinalControl and ((frame % 5) in (0, 2)): + if self.CP.openpilotLongitudinalControl and self.frame % 5 in (0, 2): target_accel = actuators.accel target_speed = max(CS.out.vEgo + (target_accel * CarControllerParams.ACCEL_TO_SPEED_MULTIPLIER), 0) max_accel = 0 if target_accel < 0 else target_accel @@ -48,17 +53,18 @@ class CarController(): # Cancel on user steering override, since there is no steering torque blending if hands_on_fault: - cruise_cancel = True + pcm_cancel_cmd = True - if ((frame % 10) == 0 and cruise_cancel): + if self.frame % 10 == 0 and pcm_cancel_cmd: # Spam every possible counter value, otherwise it might not be accepted for counter in range(16): - can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, cruise_cancel, CANBUS.chassis, counter)) - can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, cruise_cancel, CANBUS.autopilot_chassis, counter)) + can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, pcm_cancel_cmd, CANBUS.chassis, counter)) + can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, pcm_cancel_cmd, CANBUS.autopilot_chassis, counter)) # TODO: HUD control new_actuators = actuators.copy() new_actuators.steeringAngleDeg = apply_angle + self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 9b0e5ba2b7..14d8e32de4 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -71,6 +71,5 @@ class CarInterface(CarInterfaceBase): return self.CS.out def apply(self, c): - ret = self.CC.update(c, self.CS, self.frame, c.actuators, c.cruiseControl.cancel) - self.frame += 1 + ret = self.CC.update(c, self.CS) return ret diff --git a/selfdrive/car/tesla/teslacan.py b/selfdrive/car/tesla/teslacan.py index d48cd284d9..17a3b11985 100644 --- a/selfdrive/car/tesla/teslacan.py +++ b/selfdrive/car/tesla/teslacan.py @@ -1,5 +1,6 @@ import copy import crcmod + from common.conversions import Conversions as CV from selfdrive.car.tesla.values import CANBUS, CarControllerParams diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 6fa2d80dd8..c0868f3288 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -7,6 +7,7 @@ from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_comma from selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \ MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams from opendbc.can.packer import CANPacker + VisualAlert = car.CarControl.HUDControl.VisualAlert