diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 62e85033cb..f6b8e0fdff 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -1,7 +1,7 @@ from cereal import car from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.volkswagen import volkswagencan -from selfdrive.car.volkswagen.values import DBC, CANBUS, MQB_LDW_MESSAGES, BUTTON_STATES, CarControllerParams +from selfdrive.car.volkswagen.values import DBC, CANBUS, MQB_LDW_MESSAGES, BUTTON_STATES, CarControllerParams as P from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -24,64 +24,35 @@ class CarController(): def update(self, enabled, CS, frame, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart): """ Controls thread """ - P = CarControllerParams - - # Send CAN commands. can_sends = [] - #-------------------------------------------------------------------------- - # # - # Prepare HCA_01 Heading Control Assist messages with steering torque. # - # # - #-------------------------------------------------------------------------- - - # The factory camera sends at 50Hz while steering and 1Hz when not. When - # OP is active, Panda filters HCA_01 from the factory camera and OP emits - # HCA_01 at 50Hz. Rate switching creates some confusion in Cabana and - # doesn't seem to add value at this time. The rack will accept HCA_01 at - # 100Hz if we want to control at finer resolution in the future. + # **** Steering Controls ************************************************ # + if 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 + # * Don't send > 3.00 Newton-meters torque + # * Don't send the same torque for > 6 seconds + # * Don't send uninterrupted steering for > 360 seconds + # One frame of HCA disabled is enough to reset the timer, without zeroing the + # torque value. Do that anytime we happen to have 0 torque, or failing that, + # when exceeding ~1/3 the 360 second timer. - # FAULT AVOIDANCE: HCA must not be enabled at standstill. Also stop - # commanding HCA if there's a fault, so the steering rack recovers. if enabled and not (CS.out.standstill or CS.out.steerError or CS.out.steerWarning): - - # FAULT AVOIDANCE: Requested HCA torque must not exceed 3.0 Nm. This - # is inherently handled by scaling to STEER_MAX. The rack doesn't seem - # to care about up/down rate, but we have some evidence it may do its - # own rate limiting, and matching OP helps for accurate tuning. 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 - - # FAULT AVOIDANCE: HCA must not be enabled for >360 seconds. Sending - # a single frame with HCA disabled is an effective workaround. if apply_steer == 0: - # We can usually reset the timer for free, just by disabling HCA - # when apply_steer is exactly zero, which happens by chance during - # many steer torque direction changes. This could be expanded with - # a small dead-zone to capture all zero crossings, but not seeing a - # major need at this time. hcaEnabled = False self.hcaEnabledFrameCount = 0 else: self.hcaEnabledFrameCount += 1 if self.hcaEnabledFrameCount >= 118 * (100 / P.HCA_STEP): # 118s - # The Kansas I-70 Crosswind Problem: if we truly do need to steer - # in one direction for > 360 seconds, we have to disable HCA for a - # frame while actively steering. Testing shows we can just set the - # disabled flag, and keep sending non-zero torque, which keeps the - # Panda torque rate limiting safety happy. Do so 3x within the 360 - # second window for safety and redundancy. hcaEnabled = False self.hcaEnabledFrameCount = 0 else: hcaEnabled = True - # FAULT AVOIDANCE: HCA torque must not be static for > 6 seconds. - # This is to detect the sending camera being stuck or frozen. OP - # can trip this on a curve if steering is saturated. Avoid this by - # reducing torque 0.01 Nm for one frame. Do so 3x within the 6 - # second period for safety and redundancy. if self.apply_steer_last == apply_steer: self.hcaSameTorqueCount += 1 if self.hcaSameTorqueCount > 1.9 * (100 / P.HCA_STEP): # 1.9s @@ -89,9 +60,7 @@ class CarController(): self.hcaSameTorqueCount = 0 else: self.hcaSameTorqueCount = 0 - else: - # Continue sending HCA_01 messages, with the enable flags turned off. hcaEnabled = False apply_steer = 0 @@ -100,15 +69,7 @@ class CarController(): can_sends.append(volkswagencan.create_mqb_steering_control(self.packer_pt, CANBUS.pt, apply_steer, idx, hcaEnabled)) - #-------------------------------------------------------------------------- - # # - # Prepare LDW_02 HUD messages with lane borders, confidence levels, and # - # the LKAS status LED. # - # # - #-------------------------------------------------------------------------- - - # The factory camera emits this message at 10Hz. When OP is active, Panda - # filters LDW_02 from the factory camera and OP emits LDW_02 at 10Hz. + # **** HUD Controls ***************************************************** # if frame % P.LDW_STEP == 0: if visual_alert in [VisualAlert.steerRequired, VisualAlert.ldw]: @@ -116,7 +77,6 @@ class CarController(): else: hud_alert = MQB_LDW_MESSAGES["none"] - can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, CANBUS.pt, enabled, CS.out.steeringPressed, hud_alert, left_lane_visible, right_lane_visible, CS.ldw_lane_warning_left, @@ -124,17 +84,9 @@ class CarController(): CS.ldw_dlc, CS.ldw_tlc, CS.out.standstill, left_lane_depart, right_lane_depart)) - #-------------------------------------------------------------------------- - # # - # Prepare GRA_ACC_01 ACC control messages with button press events. # - # # - #-------------------------------------------------------------------------- + # **** ACC Button Controls ********************************************** # - # The car sends this message at 33hz. OP sends it on-demand only for - # virtual button presses. - # - # First create any virtual button press event needed by openpilot, to sync - # stock ACC with OP disengagement, or to auto-resume from stop. + # FIXME: this entire section is in desperate need of refactoring if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP: if not enabled and CS.out.cruiseState.enabled: @@ -148,31 +100,6 @@ class CarController(): self.graButtonStatesToSend = BUTTON_STATES.copy() self.graButtonStatesToSend["resumeCruise"] = True - # OP/Panda can see this message but can't filter it when integrated at the - # R242 LKAS camera. It could do so if integrated at the J533 gateway, but - # we need a generalized solution that works for either. The message is - # counter-protected, so we need to time our transmissions very precisely - # to achieve fast and fault-free switching between message flows accepted - # at the J428 ACC radar. - # - # Example message flow on the bus, frequency of 33Hz (GRA_ACC_STEP): - # - # CAR: 0 1 2 3 4 5 6 7 8 9 A B C D E F 0 1 2 3 4 5 6 - # EON: 3 4 5 6 7 8 9 A B C D E F 0 1 2 GG^ - # - # If OP needs to send a button press, it waits to see a GRA_ACC_01 message - # counter change, and then immediately follows up with the next increment. - # The OP message will be sent within about 1ms of the car's message, which - # is about 2ms before the car's next message is expected. OP sends for an - # arbitrary duration of 16 messages / ~0.5 sec, in lockstep with each new - # message from the car. - # - # Because OP's counter is synced to the car, J428 immediately accepts the - # OP messages as valid. Further messages from the car get discarded as - # duplicates without a fault. When OP stops sending, the extra time gap - # (GG) to the next valid car message is less than 1 * GRA_ACC_STEP. J428 - # tolerates the gap just fine and control returns to the car immediately. - if CS.graMsgBusCounter != self.graMsgBusCounterPrev: self.graMsgBusCounterPrev = CS.graMsgBusCounter if self.graButtonStatesToSend is not None: