diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py index 5714445f67..c13acebacb 100644 --- a/selfdrive/car/body/carcontroller.py +++ b/selfdrive/car/body/carcontroller.py @@ -1,8 +1,8 @@ import numpy as np from common.realtime import DT_CTRL -from selfdrive.car.body import bodycan from opendbc.can.packer import CANPacker +from selfdrive.car.body import bodycan from selfdrive.car.body.values import SPEED_FROM_RPM from selfdrive.controls.lib.pid import PIDController @@ -14,7 +14,7 @@ MAX_POS_INTEGRATOR = 0.2 # meters MAX_TURN_INTEGRATOR = 0.1 # meters -class CarController(): +class CarController: def __init__(self, dbc_name, CP, VM): self.frame = 0 self.packer = CANPacker(dbc_name) diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index c2f87a4806..d7666c1d65 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -1,8 +1,8 @@ from cereal import car from common.numpy_fast import clip, interp +from opendbc.can.packer import CANPacker from selfdrive.car.ford import fordcan from selfdrive.car.ford.values import CarControllerParams -from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -17,11 +17,12 @@ def apply_ford_steer_angle_limits(apply_steer, apply_steer_last, vEgo): return apply_steer -class CarController(): +class CarController: def __init__(self, dbc_name, CP, VM): self.CP = CP self.VM = VM self.packer = CANPacker(dbc_name) + self.frame = 0 self.apply_steer_last = 0 self.steer_rate_limited = False @@ -29,7 +30,7 @@ class CarController(): self.lkas_enabled_last = False self.steer_alert_last = False - def update(self, CC, CS, frame): + def update(self, CC, CS): can_sends = [] actuators = CC.actuators @@ -48,7 +49,7 @@ class CarController(): self.steer_rate_limited = new_steer != apply_steer # send steering commands at 20Hz - if (frame % CarControllerParams.LKAS_STEER_STEP) == 0: + if (self.frame % CarControllerParams.LKAS_STEER_STEP) == 0: lca_rq = 1 if CC.latActive else 0 # use LatCtlPath_An_Actl to actuate steering for now until curvature control is implemented @@ -69,15 +70,14 @@ class CarController(): can_sends.append(fordcan.create_tja_command(self.packer, lca_rq, ramp_type, precision, path_offset, path_angle, curvature_rate, curvature)) - send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert) # send lkas ui command at 1Hz or if ui state changes - if (frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui: + if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui: can_sends.append(fordcan.create_lkas_ui_command(self.packer, main_on, CC.latActive, steer_alert, CS.lkas_status_stock_values)) # send acc ui command at 20Hz or if ui state changes - if (frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui: + if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui: can_sends.append(fordcan.create_acc_ui_command(self.packer, main_on, CC.latActive, CS.acc_tja_status_stock_values)) self.main_on_last = main_on @@ -87,4 +87,5 @@ class CarController(): new_actuators = actuators.copy() new_actuators.steeringAngleDeg = apply_steer + self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index c608cc08d9..1af04bee41 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -78,6 +78,4 @@ class CarInterface(CarInterfaceBase): return ret def apply(self, c): - ret = self.CC.update(c, self.CS, self.frame) - self.frame += 1 - return ret + return self.CC.update(c, self.CS) diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 8ad5049e32..f763a58532 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -1,7 +1,7 @@ from cereal import car from common.conversions import Conversions as CV -from common.realtime import DT_CTRL from common.numpy_fast import interp +from common.realtime import DT_CTRL from opendbc.can.packer import CANPacker from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.gm import gmcan diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 282844d095..e0dd10d4a8 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -184,5 +184,4 @@ class CarInterface(CarInterfaceBase): return ret def apply(self, c): - ret = self.CC.update(c, self.CS) - return ret + return self.CC.update(c, self.CS) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index fb3579464a..4fbb5ce0e1 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -1,11 +1,11 @@ from cereal import car -from common.realtime import DT_CTRL -from common.numpy_fast import clip, interp from common.conversions import Conversions as CV +from common.numpy_fast import clip, interp +from common.realtime import DT_CTRL +from opendbc.can.packer import CANPacker from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.hyundai import hda2can, hyundaican from selfdrive.car.hyundai.values import Buttons, CarControllerParams, HDA2_CAR, CAR -from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert LongCtrlState = car.CarControl.Actuators.LongControlState diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 58a67f58ce..97119c77b7 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -349,5 +349,4 @@ class CarInterface(CarInterfaceBase): return ret def apply(self, c): - ret = self.CC.update(c, self.CS) - return ret + return self.CC.update(c, self.CS) diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index d003079d63..0e43a11ceb 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -1,46 +1,48 @@ from cereal import car from opendbc.can.packer import CANPacker +from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.mazda import mazdacan from selfdrive.car.mazda.values import CarControllerParams, Buttons -from selfdrive.car import apply_std_steer_torque_limits VisualAlert = car.CarControl.HUDControl.VisualAlert -class CarController(): + +class CarController: def __init__(self, dbc_name, CP, VM): self.CP = CP self.apply_steer_last = 0 self.packer = CANPacker(dbc_name) self.steer_rate_limited = False self.brake_counter = 0 + self.frame = 0 - def update(self, c, CS, frame): + def update(self, CC, CS): can_sends = [] apply_steer = 0 self.steer_rate_limited = False - if c.latActive: + if CC.latActive: # calculate steer and also set limits due to driver torque - new_steer = int(round(c.actuators.steer * CarControllerParams.STEER_MAX)) + 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 c.enabled: - if CS.out.standstill and frame % 5 == 0: + if CC.enabled: + if CS.out.standstill 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 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, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME)) - if c.cruiseControl.cancel: + 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 frame % 10 == 0 and not (CS.out.brakePressed and self.brake_counter < 7): + 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)) @@ -50,18 +52,19 @@ class CarController(): 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 + 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, - frame, apply_steer, CS.cam_lkas)) + self.frame, apply_steer, CS.cam_lkas)) - new_actuators = c.actuators.copy() + new_actuators = CC.actuators.copy() new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX + self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index cbeb910de2..35e3c1bb01 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -90,6 +90,4 @@ class CarInterface(CarInterfaceBase): return ret def apply(self, c): - ret = self.CC.update(c, self.CS, self.frame) - self.frame += 1 - return ret + return self.CC.update(c, self.CS) diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index 4ceb142d45..7d9dad6693 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -1,25 +1,27 @@ from cereal import car from common.numpy_fast import clip, interp -from selfdrive.car.nissan import nissancan from opendbc.can.packer import CANPacker +from selfdrive.car.nissan import nissancan from selfdrive.car.nissan.values import CAR, CarControllerParams - VisualAlert = car.CarControl.HUDControl.VisualAlert -class CarController(): +class CarController: def __init__(self, dbc_name, CP, VM): self.CP = CP self.car_fingerprint = CP.carFingerprint + self.frame = 0 self.lkas_max_torque = 0 self.last_angle = 0 self.packer = CANPacker(dbc_name) - def update(self, c, CS, frame, actuators, cruise_cancel, hud_alert, - left_line, right_line, left_lane_depart, right_lane_depart): + def update(self, CC, CS): + actuators = CC.actuators + hud_control = CC.hudControl + pcm_cancel_cmd = CC.cruiseControl.cancel can_sends = [] @@ -28,9 +30,9 @@ class CarController(): lkas_hud_info_msg = CS.lkas_hud_info_msg apply_angle = actuators.steeringAngleDeg - steer_hud_alert = 1 if hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0 + steer_hud_alert = 1 if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0 - if c.latActive: + if CC.latActive: # # windup slower if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle): angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_V) @@ -57,25 +59,25 @@ class CarController(): self.last_angle = apply_angle - if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA) and cruise_cancel: - can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, self.car_fingerprint, CS.cruise_throttle_msg, frame)) + if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA) and pcm_cancel_cmd: + can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, self.car_fingerprint, CS.cruise_throttle_msg, self.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 in (CAR.LEAF, CAR.LEAF_IC) and frame % 2 == 0: - can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, cruise_cancel)) + if self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC) and self.frame % 2 == 0: + can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, pcm_cancel_cmd)) can_sends.append(nissancan.create_steering_control( - self.packer, apply_angle, frame, c.enabled, self.lkas_max_torque)) + self.packer, apply_angle, self.frame, CC.enabled, self.lkas_max_torque)) if lkas_hud_msg and lkas_hud_info_msg: - if frame % 2 == 0: + if self.frame % 2 == 0: can_sends.append(nissancan.create_lkas_hud_msg( - self.packer, lkas_hud_msg, c.enabled, left_line, right_line, left_lane_depart, right_lane_depart)) + self.packer, lkas_hud_msg, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)) - if frame % 50 == 0: + if self.frame % 50 == 0: can_sends.append(nissancan.create_lkas_hud_info_msg( self.packer, lkas_hud_info_msg, steer_hud_alert )) @@ -83,4 +85,5 @@ class CarController(): new_actuators = actuators.copy() new_actuators.steeringAngleDeg = apply_angle + self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 436cef68bf..9c04d975f9 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -1,8 +1,9 @@ #!/usr/bin/env python3 from cereal import car -from selfdrive.car.nissan.values import CAR from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase +from selfdrive.car.nissan.values import CAR + class CarInterface(CarInterfaceBase): @@ -67,10 +68,4 @@ class CarInterface(CarInterfaceBase): return ret def apply(self, c): - hud_control = c.hudControl - ret = self.CC.update(c, self.CS, self.frame, c.actuators, - c.cruiseControl.cancel, hud_control.visualAlert, - hud_control.leftLaneVisible, hud_control.rightLaneVisible, - hud_control.leftLaneDepart, hud_control.rightLaneDepart) - self.frame += 1 - return ret + return self.CC.update(c, self.CS) diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index d37b82eed7..dca86c30a6 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -1,10 +1,10 @@ +from opendbc.can.packer import CANPacker from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.subaru import subarucan from selfdrive.car.subaru.values import DBC, PREGLOBAL_CARS, CarControllerParams -from opendbc.can.packer import CANPacker -class CarController(): +class CarController: def __init__(self, dbc_name, CP, VM): self.CP = CP self.apply_steer_last = 0 @@ -12,16 +12,20 @@ class CarController(): self.es_lkas_cnt = -1 self.cruise_button_prev = 0 self.steer_rate_limited = False + self.frame = 0 self.p = CarControllerParams(CP) self.packer = CANPacker(DBC[CP.carFingerprint]['pt']) - def update(self, c, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart): + def update(self, CC, CS): + actuators = CC.actuators + hud_control = CC.hudControl + pcm_cancel_cmd = CC.cruiseControl.cancel can_sends = [] # *** steering *** - if (frame % self.p.STEER_STEP) == 0: + if (self.frame % self.p.STEER_STEP) == 0: apply_steer = int(round(actuators.steer * self.p.STEER_MAX)) @@ -31,13 +35,13 @@ class CarController(): 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 c.latActive: + 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, frame, self.p.STEER_STEP)) + can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer, self.frame, self.p.STEER_STEP)) else: - can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, frame, self.p.STEER_STEP)) + can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, self.frame, self.p.STEER_STEP)) self.apply_steer_last = apply_steer @@ -70,10 +74,11 @@ class CarController(): 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, c.enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart)) + 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 diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 8c5cab86e8..952885a751 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -1,8 +1,9 @@ #!/usr/bin/env python3 from cereal import car -from selfdrive.car.subaru.values import CAR, PREGLOBAL_CARS from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase +from selfdrive.car.subaru.values import CAR, PREGLOBAL_CARS + class CarInterface(CarInterfaceBase): @@ -117,9 +118,4 @@ class CarInterface(CarInterfaceBase): return ret def apply(self, c): - hud_control = c.hudControl - ret = self.CC.update(c, self.CS, self.frame, c.actuators, - c.cruiseControl.cancel, hud_control.visualAlert, - hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart) - self.frame += 1 - return ret + return self.CC.update(c, self.CS) diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index d4eda38e64..5e78c72b60 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -64,5 +64,4 @@ class CarInterface(CarInterfaceBase): return ret def apply(self, c): - ret = self.CC.update(c, self.CS) - return ret + return self.CC.update(c, self.CS) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index fdb923f693..337c039565 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -274,5 +274,4 @@ class CarInterface(CarInterfaceBase): # pass in a car.CarControl # to be called @ 100hz def apply(self, c): - ret = self.CC.update(c, self.CS) - return ret + return self.CC.update(c, self.CS) diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 842eb6d139..4614463c6e 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -1,15 +1,17 @@ from cereal import car +from opendbc.can.packer import CANPacker from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.volkswagen import volkswagencan from selfdrive.car.volkswagen.values import DBC_FILES, CANBUS, MQB_LDW_MESSAGES, BUTTON_STATES, CarControllerParams as P -from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert -class CarController(): + +class CarController: def __init__(self, dbc_name, CP, VM): - self.apply_steer_last = 0 self.CP = CP + self.apply_steer_last = 0 + self.frame = 0 self.packer_pt = CANPacker(DBC_FILES.mqb) @@ -22,14 +24,15 @@ class CarController(): self.steer_rate_limited = False - def update(self, c, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart): - """ Controls thread """ + def update(self, CC, CS, ext_bus): + actuators = CC.actuators + hud_control = CC.hudControl can_sends = [] # **** Steering Controls ************************************************ # - if frame % P.HCA_STEP == 0: + if self.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 @@ -40,7 +43,7 @@ class CarController(): # torque value. Do that anytime we happen to have 0 torque, or failing that, # when exceeding ~1/3 the 360 second timer. - if c.latActive: + if CC.latActive: 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 @@ -66,34 +69,34 @@ class CarController(): apply_steer = 0 self.apply_steer_last = apply_steer - idx = (frame / P.HCA_STEP) % 16 + idx = (self.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): + if self.frame % P.LDW_STEP == 0: + if hud_control.visualAlert 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, c.enabled, - CS.out.steeringPressed, hud_alert, left_lane_visible, - right_lane_visible, CS.ldw_stock_values, - left_lane_depart, right_lane_depart)) + can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, CANBUS.pt, CC.enabled, + CS.out.steeringPressed, hud_alert, hud_control.leftLaneVisible, + hud_control.rightLaneVisible, CS.ldw_stock_values, + hud_control.leftLaneDepart, hud_control.rightLaneDepart)) # **** ACC Button Controls ********************************************** # # FIXME: this entire section is in desperate need of refactoring if self.CP.pcmCruise: - if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP: - if c.cruiseControl.cancel: + if self.frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP: + if CC.cruiseControl.cancel: # Cancel ACC if it's engaged with OP disengaged. self.graButtonStatesToSend = BUTTON_STATES.copy() self.graButtonStatesToSend["cancel"] = True - elif c.enabled and CS.out.cruiseState.standstill: + elif CC.enabled and CS.out.cruiseState.standstill: # Blip the Resume button if we're engaged at standstill. # FIXME: This is a naive implementation, improve with visiond or radar input. self.graButtonStatesToSend = BUTTON_STATES.copy() @@ -103,7 +106,7 @@ class CarController(): self.graMsgBusCounterPrev = CS.graMsgBusCounter if self.graButtonStatesToSend is not None: if self.graMsgSentCount == 0: - self.graMsgStartFramePrev = frame + self.graMsgStartFramePrev = self.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 @@ -114,4 +117,5 @@ class CarController(): new_actuators = actuators.copy() new_actuators.steer = self.apply_steer_last / P.STEER_MAX + self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 6782105c16..c2b077b6db 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -193,12 +193,4 @@ class CarInterface(CarInterfaceBase): return ret def apply(self, c): - hud_control = c.hudControl - ret = self.CC.update(c, self.CS, self.frame, self.ext_bus, c.actuators, - hud_control.visualAlert, - hud_control.leftLaneVisible, - hud_control.rightLaneVisible, - hud_control.leftLaneDepart, - hud_control.rightLaneDepart) - self.frame += 1 - return ret + return self.CC.update(c, self.CS, self.ext_bus)