Merge pull request commaai#407 from arne182/mapd

Mapd options under /data/kegman.json and fixes for incomplete roundabouts.
priuscom · Jul 14, 2019 · 6d058de · 6d058de
2 parents 14aaeb8 + 127b160
commit 6d058de
Show file tree

Hide file tree

Showing 4 changed files with 6 additions and 4 deletions.
diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py
@@ -165,9 +165,9 @@ def update(self, rcv_times, CS, CP, VM, PP, live20, live100, md, live_map_data):
     map_valid = True #live_map_data.liveMapData.mapValid and map_age < 10.0
 
     # Speed limit and curvature
-    set_speed_limit_active = self.params.get("LimitSetSpeed") == "1" and self.params.get("SpeedLimitOffset") is not None
+    set_speed_limit_active = kegman.get("LimitSetSpeed")
     if set_speed_limit_active and map_valid:
-      offset = float(self.params.get("SpeedLimitOffset"))
+      offset = float(kegman.get("SpeedLimitOffset"))
       if live_map_data.liveMapData.speedLimitValid:
         speed_limit = live_map_data.liveMapData.speedLimit
         v_speedlimit = speed_limit + offset

diff --git a/selfdrive/kegman_conf.py b/selfdrive/kegman_conf.py
@@ -11,7 +11,7 @@ def read_config():
                     "wheelTouchSeconds": 1800, "battPercOff": 25, "carVoltageMinEonShutdown": 11200,
                     "brakeStoppingTarget": 0.25, "angle_steers_offset": 0, "brake_distance_extra": 1,
                     "lastALCAMode": 1, "brakefactor": 1.2, "lastGasMode": 0, "lastSloMode": 1,
-                    "leadDistance": 5, "useCarCaching": True, "autoUpdate": True}
+                    "leadDistance": 5, "useCarCaching": True, "autoUpdate": True, "SpeedLimitOffset": 0, "LimitSetSpeed": True}
 
   if os.path.isfile(kegman_file):
     try:

diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py
@@ -1,6 +1,7 @@
 #!/usr/bin/env python
 
 # Add phonelibs openblas to LD_LIBRARY_PATH if import fails
+import selfdrive.crash as crash
 from common.basedir import BASEDIR
 try:
   from scipy import spatial
@@ -121,6 +122,7 @@ def query_thread():
 
       except Exception as e:
         print(e)
+        crash.capture_warning(e)
         query_lock.acquire()
         last_query_result = None
         query_lock.release()

diff --git a/selfdrive/mapd/mapd_helpers.py b/selfdrive/mapd/mapd_helpers.py
@@ -253,7 +253,7 @@ def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead):
             a = 111132.954*math.cos(float(latmax+latmin)/360*3.141592)*float(lonmax-lonmin)
           else:
             circle = circle_through_points([way.way.nodes[0].lat,way.way.nodes[0].lon,1], [way.way.nodes[1].lat,way.way.nodes[1].lon,1], [way.way.nodes[-1].lat,way.way.nodes[-1].lon,1])
-            a = 111132.954*math.cos(float(latmax+latmin)/360*3.141592)*float(circle[2])
+            a = 111132.954*math.cos(float(latmax+latmin)/360*3.141592)*float(circle[2])*2
           speed_ahead = np.sqrt(1.6075*a)
           min_dist = 999.9
           for w in way_pts: