Merge pull request #27 from emmertex/community-devel

Merge Devel Branch

.gitignore

@@ -30,9 +30,8 @@ selfdrive/proclogd/proclogd
 selfdrive/ui/ui
 selfdrive/test/tests/plant/out
 /src/
-
+openpilot_tools
 one
-
+selfdrive/visiond/visiond
 .vscode/*.json
-launch_chffrplus.sh
-
+launch_chffrplus.sh

selfdrive/car/hyundai/carcontroller.py

@@ -54,9 +54,9 @@ def update(self, sendcan, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
       return
 
     if self.checksum == "NONE":
-        self.checksum = learn_checksum(self.packer, CS.lkas11)
-        if self.checksum == "NONE":
-            return
+      self.checksum = learn_checksum(self.packer, CS.lkas11)
+      if self.checksum == "NONE":
+        return
 
     force_enable = False
 
@@ -66,8 +66,8 @@ def update(self, sendcan, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
       force_enable = True
 
     if (self.car_fingerprint in FEATURES["soft_disable"] and CS.v_wheel < 16.8):
-        enabled = False
-        force_enable = False
+      enabled = False
+      force_enable = False
 
 
     if (CS.left_blinker_on == 1 or CS.right_blinker_on == 1):
@@ -139,38 +139,38 @@ def update(self, sendcan, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
     # Run this twice a second
     if (self.cnt % 50) == 0:
       if self.params.get("LimitSetSpeed") == "1" and self.params.get("SpeedLimitOffset") is not None:
-          # If Not Enabled, or cruise not set, allow auto speed adjustment again
-          if not (enabled and CS.acc_active_real):
+        # If Not Enabled, or cruise not set, allow auto speed adjustment again
+        if not (enabled and CS.acc_active_real):
+          self.speed_adjusted = False
+        # Attempt to read the speed limit from zmq
+        map_data = messaging.recv_one_or_none(self.map_data_sock)
+        # If we got a message
+        if map_data != None:
+          # See if we use Metric or dead kings ligaments for measurements, and set a variable to the conversion value
+          if bool(self.params.get("IsMetric")):
+            self.speed_conv = CV.MS_TO_KPH
+          else:
+            self.speed_conv = CV.MS_TO_MPH
+
+          # If the speed limit is valid
+          if map_data.liveMapData.speedLimitValid:
+            last_speed = self.map_speed
+            # Get the speed limit, and add the offset to it,
+            v_speed = (map_data.liveMapData.speedLimit + float(self.params.get("SpeedLimitOffset")))
+            ## Stolen curvature code from planner.py, and updated it for us
+            v_curvature = 45.0
+            if map_data.liveMapData.curvatureValid:
+              v_curvature = math.sqrt(1.85 / max(1e-4, abs(map_data.liveMapData.curvature)))
+            # Use the minimum between Speed Limit and Curve Limit, and convert it as needed
+            self.map_speed = min(v_speed, v_curvature) * self.speed_conv
+            # Compare it to the last time the speed was read.  If it is different, set the flag to allow it to auto set our speed
+            if last_speed != self.map_speed:
               self.speed_adjusted = False
-          # Attempt to read the speed limit from zmq
-          map_data = messaging.recv_one_or_none(self.map_data_sock)
-          # If we got a message
-          if map_data != None:
-            # See if we use Metric or dead kings ligaments for measurements, and set a variable to the conversion value
-            if bool(self.params.get("IsMetric")):
-              self.speed_conv = CV.MS_TO_KPH
-            else:
-              self.speed_conv = CV.MS_TO_MPH
-
-            # If the speed limit is valid
-            if map_data.liveMapData.speedLimitValid:
-              last_speed = self.map_speed
-              # Get the speed limit, and add the offset to it,
-              v_speed = (map_data.liveMapData.speedLimit + float(self.params.get("SpeedLimitOffset")))
-              ## Stolen curvature code from planner.py, and updated it for us
-              v_curvature = 45.0
-              if map_data.liveMapData.curvatureValid:
-                v_curvature = math.sqrt(1.85 / max(1e-4, abs(map_data.liveMapData.curvature)))
-              # Use the minimum between Speed Limit and Curve Limit, and convert it as needed
-              self.map_speed = min(v_speed, v_curvature) * self.speed_conv
-              # Compare it to the last time the speed was read.  If it is different, set the flag to allow it to auto set our speed
-              if last_speed != self.map_speed:
-                  self.speed_adjusted = False
-              print self.map_speed
-            else:
-              # If it is not valid, set the flag so the cruise speed won't be changed.
-              self.map_speed = 0
-              self.speed_adjusted = True
+            print self.map_speed
+          else:
+            # If it is not valid, set the flag so the cruise speed won't be changed.
+            self.map_speed = 0
+            self.speed_adjusted = True
       else:
         self.speed_adjusted = True
 
@@ -180,14 +180,14 @@ def update(self, sendcan, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
     # Do the spamming 10 times a second, we might get from 0 to 10 successful
     # Only do this if we have not yet set the cruise speed
     if CS.acc_active_real and not self.speed_adjusted and self.map_speed > (8.5 * self.speed_conv) and (self.cnt % 9 == 0 or self.cnt % 9 == 1):
-        # Use some tolerance because of Floats being what they are...
-        if (CS.cruise_set_speed * self.speed_conv) > (self.map_speed * 1.005):
-            can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.SET_DECEL, (1 if self.cnt % 9 == 1 else 0)))
-        elif (CS.cruise_set_speed * self.speed_conv) < (self.map_speed / 1.005):
-            can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL, (1 if self.cnt % 9 == 1 else 0)))
-        # If nothing needed adjusting, then the speed has been set, which will lock out this control
-        else:
-            self.speed_adjusted = True
+      # Use some tolerance because of Floats being what they are...
+      if (CS.cruise_set_speed * self.speed_conv) > (self.map_speed * 1.005):
+        can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.SET_DECEL, (1 if self.cnt % 9 == 1 else 0)))
+      elif (CS.cruise_set_speed * self.speed_conv) < (self.map_speed / 1.005):
+        can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL, (1 if self.cnt % 9 == 1 else 0)))
+      # If nothing needed adjusting, then the speed has been set, which will lock out this control
+      else:
+        self.speed_adjusted = True
 
     ### Send messages to canbus
     sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())

selfdrive/car/hyundai/carstate.py

@@ -242,12 +242,12 @@ def update(self, cp, cp_cam, cp_cam2):
     self.park_brake = cp.vl["CGW1"]['CF_Gway_ParkBrakeSw']
     self.main_on = True
     if self.has_scc:
-        self.acc_active = (cp.vl["SCC12"]['ACCMode'] != 0) if not self.cstm_btns.get_button_status("alwon") else \
+      self.acc_active = (cp.vl["SCC12"]['ACCMode'] != 0) if not self.cstm_btns.get_button_status("alwon") else \
             (cp.vl["SCC11"]["MainMode_ACC"] != 0)  # I'm Dangerous!
-        self.acc_active_real = (cp.vl["SCC12"]['ACCMode'] !=0)
+      self.acc_active_real = (cp.vl["SCC12"]['ACCMode'] !=0)
     else:
-        self.acc_active = (cp.vl["LVR12"]['CF_Lvr_CruiseSet'] != 0)
-        self.acc_active_real = self.acc_active
+      self.acc_active = (cp.vl["LVR12"]['CF_Lvr_CruiseSet'] != 0)
+      self.acc_active_real = self.acc_active
     self.pcm_acc_status = int(self.acc_active)
 
     # calc best v_ego estimate, by averaging two opposite corners
@@ -326,13 +326,13 @@ def update(self, cp, cp_cam, cp_cam2):
     # Gear Selecton via TCU12
     gear2 = cp.vl["TCU12"]["CUR_GR"]
     if gear2 == 0:
-        self.gear_tcu = "park"
+      self.gear_tcu = "park"
     elif gear2 == 14:
-        self.gear_tcu = "reverse"
+      self.gear_tcu = "reverse"
     elif gear2 > 0 and gear2 < 14:    # unaware of anything over 8 currently
-        self.gear_tcu = "drive"
+      self.gear_tcu = "drive"
     else:
-        self.gear_tcu = "unknown"
+      self.gear_tcu = "unknown"
 
     # save the entire LKAS11, CLU11 and MDPS12
     if cp_cam.can_valid == True:

selfdrive/car/hyundai/hyundaican.py

@@ -14,7 +14,8 @@ def create_lkas11(packer, car_fingerprint, apply_steer, steer_req, cnt, \
   values = {
     "CF_Lkas_Icon": 2 if (car_fingerprint in FEATURES["icon_basic"]) else \
         (lkas11["CF_Lkas_Icon"] if use_stock else (2 if enabled else 0)),
-    "CF_Lkas_LdwsSysState": lkas11["CF_Lkas_LdwsSysState"] if use_stock else 3,
+    "CF_Lkas_LdwsSysState": lkas11["CF_Lkas_LdwsSysState"] if use_stock else \
+        (4 if enabled else lkas11["CF_Lkas_LdwsSysState"]),
     "CF_Lkas_SysWarning": lkas11["CF_Lkas_SysWarning"] if use_stock else hud_alert,
     "CF_Lkas_LdwsLHWarning": lkas11["CF_Lkas_LdwsLHWarning"] if keep_stock else 0,
     "CF_Lkas_LdwsRHWarning": lkas11["CF_Lkas_LdwsRHWarning"] if keep_stock else 0,
@@ -97,28 +98,28 @@ def create_mdps12(packer, car_fingerprint, cnt, mdps12, lkas11, camcan, checksum
 def learn_checksum(packer, lkas11):
     # Learn checksum used
     values = {
-        "CF_Lkas_Icon": lkas11["CF_Lkas_Icon"],
-        "CF_Lkas_LdwsSysState": lkas11["CF_Lkas_LdwsSysState"],
-        "CF_Lkas_SysWarning": lkas11["CF_Lkas_SysWarning"],
-        "CF_Lkas_LdwsLHWarning": lkas11["CF_Lkas_LdwsLHWarning"],
-        "CF_Lkas_LdwsRHWarning": lkas11["CF_Lkas_LdwsRHWarning"],
-        "CF_Lkas_HbaLamp": lkas11["CF_Lkas_HbaLamp"],
-        "CF_Lkas_FcwBasReq": lkas11["CF_Lkas_FcwBasReq"],
-        "CR_Lkas_StrToqReq": lkas11["CR_Lkas_StrToqReq"],
-        "CF_Lkas_ActToi": lkas11["CF_Lkas_ActToi"],
-        "CF_Lkas_ToiFlt": lkas11["CF_Lkas_ToiFlt"],
-        "CF_Lkas_HbaSysState": lkas11["CF_Lkas_HbaSysState"],
-        "CF_Lkas_FcwOpt": lkas11["CF_Lkas_FcwOpt"],
-        "CF_Lkas_HbaOpt": lkas11["CF_Lkas_HbaOpt"],
-        "CF_Lkas_MsgCount": lkas11["CF_Lkas_MsgCount"],
-        "CF_Lkas_FcwSysState": lkas11["CF_Lkas_FcwSysState"],
-        "CF_Lkas_FcwCollisionWarning": lkas11["CF_Lkas_FcwCollisionWarning"],
-        "CF_Lkas_FusionState": lkas11["CF_Lkas_FusionState"],
-        "CF_Lkas_Chksum": lkas11["CF_Lkas_Chksum"],
-        "CF_Lkas_FcwOpt_USM": lkas11["CF_Lkas_FcwOpt_USM"],
-        "CF_Lkas_LdwsOpt_USM": lkas11["CF_Lkas_LdwsOpt_USM"],
-        "CF_Lkas_Unknown1": lkas11["CF_Lkas_Unknown1"],
-        "CF_Lkas_Unknown2": lkas11["CF_Lkas_Unknown2"],
+      "CF_Lkas_Icon": lkas11["CF_Lkas_Icon"],
+      "CF_Lkas_LdwsSysState": lkas11["CF_Lkas_LdwsSysState"],
+      "CF_Lkas_SysWarning": lkas11["CF_Lkas_SysWarning"],
+      "CF_Lkas_LdwsLHWarning": lkas11["CF_Lkas_LdwsLHWarning"],
+      "CF_Lkas_LdwsRHWarning": lkas11["CF_Lkas_LdwsRHWarning"],
+      "CF_Lkas_HbaLamp": lkas11["CF_Lkas_HbaLamp"],
+      "CF_Lkas_FcwBasReq": lkas11["CF_Lkas_FcwBasReq"],
+      "CR_Lkas_StrToqReq": lkas11["CR_Lkas_StrToqReq"],
+      "CF_Lkas_ActToi": lkas11["CF_Lkas_ActToi"],
+      "CF_Lkas_ToiFlt": lkas11["CF_Lkas_ToiFlt"],
+      "CF_Lkas_HbaSysState": lkas11["CF_Lkas_HbaSysState"],
+      "CF_Lkas_FcwOpt": lkas11["CF_Lkas_FcwOpt"],
+      "CF_Lkas_HbaOpt": lkas11["CF_Lkas_HbaOpt"],
+      "CF_Lkas_MsgCount": lkas11["CF_Lkas_MsgCount"],
+      "CF_Lkas_FcwSysState": lkas11["CF_Lkas_FcwSysState"],
+      "CF_Lkas_FcwCollisionWarning": lkas11["CF_Lkas_FcwCollisionWarning"],
+      "CF_Lkas_FusionState": lkas11["CF_Lkas_FusionState"],
+      "CF_Lkas_Chksum": lkas11["CF_Lkas_Chksum"],
+      "CF_Lkas_FcwOpt_USM": lkas11["CF_Lkas_FcwOpt_USM"],
+      "CF_Lkas_LdwsOpt_USM": lkas11["CF_Lkas_LdwsOpt_USM"],
+      "CF_Lkas_Unknown1": lkas11["CF_Lkas_Unknown1"],
+      "CF_Lkas_Unknown2": lkas11["CF_Lkas_Unknown2"],
     }
 
     dat = packer.make_can_msg("LKAS11", 0, values)[2]
@@ -133,11 +134,11 @@ def learn_checksum(packer, lkas11):
     cs7b = ((sum(dat[:6]) + dat[7]) % 256)
 
     if cs6b != crc and cs7b != crc and cs6b != cs7b:
-        if crc == lkas11["CF_Lkas_Chksum"]:
-            return "crc8"
-        elif cs6b == lkas11["CF_Lkas_Chksum"]:
-            return "6B"
-        elif cs7b == lkas11["CF_Lkas_Chksum"]:
-            return "7B"
+      if crc == lkas11["CF_Lkas_Chksum"]:
+        return "crc8"
+      elif cs6b == lkas11["CF_Lkas_Chksum"]:
+        return "6B"
+      elif cs7b == lkas11["CF_Lkas_Chksum"]:
+        return "7B"
 
     return "NONE"

selfdrive/controls/lib/latcontrol.py

@@ -82,6 +82,7 @@ def __init__(self, CP):
     self.rough_steers_rate_increment = 0.0
     self.prev_angle_steers = 0.0
     self.calculate_rate = True
+    self.sat_time = 0.0
 
     # variables for dashboarding
     self.context = zmq.Context()
@@ -277,7 +278,23 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly
         PL.PP.d_poly[0], PL.PP.d_poly[1], PL.PP.d_poly[2], PL.PP.lane_width, PL.PP.lane_width_estimate, PL.PP.lane_width_certainty, v_ego, \
         self.pid.p, self.pid.i, self.pid.f, self.curvature_factor, VM.gsfc, VM.curvf, VM.sf, int(time.time() * 100) * 10000000))
 
-    self.sat_flag = self.pid.saturated
+    # Reset sat_flat always, set it only if needed
+    self.sat_flag = False
+
+    # If PID is saturated, set time which it was saturated
+    if self.pid.saturated and self.sat_time < 0.5:
+      self.sat_time = sec_since_boot()
+
+    # To save cycles, nest in sat_time check
+    if self.sat_time > 0.5:
+      # If its been saturated for 1.5 seconds then set flag
+      if (sec_since_boot() - self.sat_time) > 0.7:
+        self.sat_flag = True
+
+      # If it is no longer saturated, clear the sat flag and timer
+      if not self.pid.saturated:
+        self.sat_time = 0.0
+
     self.prev_angle_rate = angle_rate
     self.prev_angle_steers = angle_steers