Merge pull request commaai#2 from arne182/dynamic-follow

Dynamic follow

cereal/car.capnp

@@ -123,6 +123,7 @@ struct CarState {
   blindspotside @30 :Float32;
   readdistancelines @31 :Float32;
   gasbuttonstatus @32 :Float32;
+  lkMode @33 :Bool;
 
   # lock info
   doorOpen @24 :Bool;

selfdrive/car/gm/carcontroller.py

@@ -114,7 +114,7 @@ def update(self, sendcan, enabled, CS, frame, actuators, \
     ### STEER ###
 
     if (frame % P.STEER_STEP) == 0:
-      lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > 3.
+      lkas_enabled = enabled and not CS.steer_not_allowed and CS.lkMode and CS.v_ego > 3.
       if lkas_enabled:
         apply_steer = alca_steer * P.STEER_MAX
         apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, P)

selfdrive/car/gm/carstate.py

@@ -26,6 +26,7 @@ def get_powertrain_can_parser(CP, canbus):
     ("TurnSignals", "BCMTurnSignals", 0),
     ("AcceleratorPedal", "AcceleratorPedal", 0),
     ("ACCButtons", "ASCMSteeringButton", CruiseButtons.UNPRESS),
+    ("LKAButton", "ASCMSteeringButton", 0),
     ("SteeringWheelAngle", "PSCMSteeringAngle", 0),
     ("FLWheelSpd", "EBCMWheelSpdFront", 0),
     ("FRWheelSpd", "EBCMWheelSpdFront", 0),
@@ -71,7 +72,10 @@ def __init__(self, CP, canbus):
     self.prev_left_blinker_on = False
     self.right_blinker_on = False
     self.prev_right_blinker_on = False
-    self.follow_level = 3
+    self.follow_level = 4
+    self.prev_lka_button = 0
+    self.lka_button = 0
+    self.lkMode = True
 
     # ALCA PARAMS
     self.blind_spot_on = bool(0)
@@ -187,6 +191,9 @@ def update(self, pt_cp):
     self.v_ego = float(v_ego_x[0])
     self.a_ego = float(v_ego_x[1])
 
+    self.prev_lka_button = self.lka_button
+    self.lka_button = pt_cp.vl["ASCMSteeringButton"]['LKAButton']
+
     self.standstill = self.v_ego_raw < 0.01
 
     self.angle_steers = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle']

selfdrive/car/gm/interface.py

@@ -274,14 +274,25 @@ def update(self, c):
           be.type = 'accelCruise' # Suppress resume button if we're resuming from stop so we don't adjust speed.
       elif but == CruiseButtons.DECEL_SET:
         be.type = 'decelCruise'
+        if not cruiseEnabled and not self.CS.lkMode:
+          self.CS.lkMode = True
       elif but == CruiseButtons.CANCEL:
         be.type = 'cancel'
       elif but == CruiseButtons.MAIN:
         be.type = 'altButton3'
       buttonEvents.append(be)
 
     ret.buttonEvents = buttonEvents
+
+    if self.CS.lka_button and self.CS.lka_button != self.CS.prev_lka_button:
+      if self.CS.lkMode:
+        self.CS.lkMode = False
+      else:
+        self.CS.lkMode = True
+
     if self.CS.distance_button and self.CS.distance_button != self.CS.prev_distance_button:
+      if self.CS.follow_level == 4:
+        self.CS.follow_level -= 1
       self.CS.follow_level -= 1
       if self.CS.follow_level < 1:
         self.CS.follow_level = 3

selfdrive/car/honda/carstate.py

@@ -156,9 +156,9 @@ def __init__(self, CP):
     self.alcaLabels = ["MadMax","Normal","Wifey"]
     self.visionLabels = ["normal","wiggly"]
     self.visionMode = 0
-    self.trLabels = ["0.9","1.8","2.7"]
+    self.trLabels = ["0.9","1.8","2.7","dyn"]
     self.alcaMode = 0
-    self.trMode = 1
+    self.trMode = 3
     #if (CP.carFingerprint == CAR.MODELS):
     # ALCA PARAMS
     # max REAL delta angle for correction vs actuator
@@ -197,7 +197,7 @@ def __init__(self, CP):
     self.CL_WAIT_BEFORE_START = 1
     #END OF ALCA PARAMS
 
-    self.read_distance_lines_prev = 4
+    self.read_distance_lines_prev = 3
 
     self.CP = CP
     self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
@@ -276,7 +276,7 @@ def update_ui_buttons(self,id,btn_status):
 
       elif (id == 4) and (btn_status == 0) and self.cstm_btns.btns[id].btn_name=="tr":
           if self.cstm_btns.btns[id].btn_label2 == self.trLabels[self.trMode]:
-            self.trMode = (self.trMode + 1 ) % 3
+            self.trMode = (self.trMode + 1 ) % 4
           else:
             self.trMode = 0
           self.cstm_btns.btns[id].btn_label2 = self.trLabels[self.trMode]
@@ -421,6 +421,8 @@ def update(self, cp, cp_cam):
     self.hud_lead = cp.vl["ACC_HUD"]['HUD_LEAD']
     if self.cruise_setting == 3:
       if cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"] == 0:
+        if self.trMode == 3:
+          self.trMode = 1
         self.trMode = (self.trMode + 1 ) % 3
         self.cstm_btns.btns[4].btn_label2 = self.trLabels[self.trMode]
     self.prev_cruise_setting = self.cruise_setting

selfdrive/car/toyota/carstate.py

@@ -128,6 +128,7 @@ def get_cam_can_parser(CP):
 
 class CarState(object):
   def __init__(self, CP):
+    self.trfix = False
     self.Angles = np.zeros(250)
     self.Angles_later = np.zeros(250)
     self.Angle_counter = 0
@@ -196,7 +197,7 @@ def __init__(self, CP):
     self.includeradius = 22
     self.blind_spot_on = bool(0)
     self.distance_toggle_prev = 2
-    self.read_distance_lines_prev = 4
+    self.read_distance_lines_prev = 3
     self.lane_departure_toggle_on_prev = True
     self.acc_slow_on_prev = False
     #BB UIEvents
@@ -361,7 +362,12 @@ def update(self, cp, cp_cam):
         self.lane_departure_toggle_on = True
 
     self.distance_toggle = cp.vl["JOEL_ID"]['ACC_DISTANCE']
-    self.read_distance_lines = cp.vl["PCM_CRUISE_SM"]['DISTANCE_LINES']
+    if cp.vl["PCM_CRUISE_SM"]['DISTANCE_LINES'] == 2:
+      self.trfix = True
+    if self.trfix:
+      self.read_distance_lines = cp.vl["PCM_CRUISE_SM"]['DISTANCE_LINES']
+    else:
+      self.read_distance_lines = 4
     if self.distance_toggle <> self.distance_toggle_prev:
       if self.read_distance_lines == self.distance_toggle:
         self.distance_toggle_prev = self.distance_toggle