From 2db099a179dca79315c818e5b3477ae7694abc57 Mon Sep 17 00:00:00 2001
From: Jason Young <jyoung8607@gmail.com>
Date: Mon, 22 Nov 2021 17:14:13 -0500
Subject: [PATCH] don't run long state/actuators unless sending message

---
 selfdrive/car/volkswagen/carcontroller.py | 73 +++++++++++------------
 1 file changed, 36 insertions(+), 37 deletions(-)

diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py
index 5a101f63ab..cce1d7f975 100644
--- a/selfdrive/car/volkswagen/carcontroller.py
+++ b/selfdrive/car/volkswagen/carcontroller.py
@@ -35,40 +35,40 @@ class CarController():
     # **** Acceleration and Braking Controls ******************************** #
 
     if CS.CP.openpilotLongitudinalControl:
-      if CS.tsk_status in [2, 3, 4, 5]:
-        acc_status = 3 if enabled else 2
-      else:
-        acc_status = CS.tsk_status
-
-      accel = actuators.accel if enabled else 0
-      accel = clip(accel, P.ACCEL_MIN, P.ACCEL_MAX) if enabled else 0
-
-      # FIXME: this needs to become a proper state machine
-      acc_hold_request, acc_hold_release = False, False
-      if actuators.longControlState == LongCtrlState.stopping:
-        self.acc_stopping = True
-        acc_hold_request = not CS.esp_hold_confirmation  # Send hold request for ABS/ESP brake-hold until acked
-      elif enabled:
-        if self.acc_stopping:
-          self.acc_starting = True
-          self.acc_stopping = False
-        self.acc_starting &= CS.out.vEgo < CS.CP.vEgoStarting
-        acc_hold_release = CS.esp_hold_confirmation  # Send release request for ABS/ESP brake-hold until acked
-      else:
-        self.acc_stopping, self.acc_starting = False, False
+      if frame % P.ACC_CONTROL_STEP == 0:
+        if CS.tsk_status in [2, 3, 4, 5]:
+          acc_status = 3 if enabled else 2
+        else:
+          acc_status = CS.tsk_status
+
+        accel = actuators.accel if enabled else 0
+        accel = clip(accel, P.ACCEL_MIN, P.ACCEL_MAX) if enabled else 0
+
+        # FIXME: this needs to become a proper state machine
+        acc_hold_request, acc_hold_release = False, False
+        if actuators.longControlState == LongCtrlState.stopping:
+          self.acc_stopping = True
+          acc_hold_request = not CS.esp_hold_confirmation  # Send hold request for ABS/ESP brake-hold until acked
+        elif enabled:
+          if self.acc_stopping:
+            self.acc_starting = True
+            self.acc_stopping = False
+          self.acc_starting &= CS.out.vEgo < CS.CP.vEgoStarting
+          acc_hold_release = CS.esp_hold_confirmation  # Send release request for ABS/ESP brake-hold until acked
+        else:
+          self.acc_stopping, self.acc_starting = False, False
 
-      # Hopefully, have PI yank the lag out and let the drivetrain coordinator make it smooth again
-      cb_pos = 0.2
-      cb_neg = 0.2
+        # Hopefully, have PI yank the lag out and let the drivetrain coordinator make it smooth again
+        cb_pos = 0.2
+        cb_neg = 0.2
 
-      if acc_hold_request:
-        weird_value = 0x88
-      elif self.acc_stopping:
-        weird_value = 0x95
-      else:
-        weird_value = 0x7F
+        if acc_hold_request:
+          weird_value = 0x88
+        elif self.acc_stopping:
+          weird_value = 0x95
+        else:
+          weird_value = 0x7F
 
-      if frame % P.ACC_CONTROL_STEP == 0:
         idx = (frame / P.ACC_CONTROL_STEP) % 16
         can_sends.append(volkswagencan.create_mqb_acc_06_control(self.packer_pt, CANBUS.pt, enabled, acc_status,
                                                                  accel, self.acc_stopping, self.acc_starting,
@@ -77,6 +77,11 @@ class CarController():
                                                                  accel, self.acc_stopping, self.acc_starting,
                                                                  acc_hold_request, acc_hold_release, weird_value, idx))
 
+      if frame % P.ACC_HUD_STEP == 0:
+        idx = (frame / P.ACC_HUD_STEP) % 16
+        can_sends.append(volkswagencan.create_mqb_acc_hud_control(self.packer_pt, CANBUS.pt, CS.tsk_status,
+                                                                  set_speed * CV.MS_TO_KPH, idx))
+
     # **** Steering Controls ************************************************ #
 
     if frame % P.HCA_STEP == 0:
@@ -133,12 +138,6 @@ class CarController():
                                                             right_lane_visible, CS.ldw_stock_values,
                                                             left_lane_depart, right_lane_depart))
 
-    if CS.CP.openpilotLongitudinalControl:
-      if frame % P.ACC_HUD_STEP == 0:
-        idx = (frame / P.ACC_HUD_STEP) % 16
-        can_sends.append(volkswagencan.create_mqb_acc_hud_control(self.packer_pt, CANBUS.pt, CS.tsk_status,
-                                                                  set_speed * CV.MS_TO_KPH, idx))
-
     # **** ACC Button Controls ********************************************** #
 
     # FIXME: this entire section is in desperate need of refactoring