Merge pull request commaai#62 from arne182/release2

update to latest

cereal/log.capnp

@@ -1639,7 +1639,12 @@ struct LiveMapData {
   distToTurn @10 :Float32;
   mapValid @11 :Bool;
 }
-
+struct LiveTrafficData {
+  speedLimitValid @0 :Bool;
+  speedLimit @1 :Float32;
+  speedAdvisoryValid @2 :Bool;
+  speedAdvisory @3 :Float32;
+}  
 struct LatControl {
   anglelater @0 :Float32;
 }
@@ -1729,5 +1734,6 @@ struct Event {
     cameraOdometry @64 :CameraOdometry;
     pathPlan @65 :PathPlan;
     kalmanOdometry @66 :KalmanOdometry;
+    liveTrafficData @67 :LiveTrafficData;
   }
 }

selfdrive/car/toyota/carstate.py

@@ -206,6 +206,10 @@ def __init__(self, CP):
     #gps_ext_sock = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, poller)
     self.gps_location = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, conflate=True, poller=self.poller)
     self.lat_Control = messaging.sub_sock(context, service_list['latControl'].port, conflate=True, poller=self.poller)
+    self.live_MapData = messaging.sub_sock(context, service_list['liveMapData'].port, conflate=True, poller=self.poller)
+    self.traffic_data_sock = messaging.pub_sock(context, service_list['liveTrafficData'].port)
+
+    self.spdval1 = 0
     self.CP = CP
     self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
     self.shifter_values = self.can_define.dv["GEAR_PACKET"]['GEAR']
@@ -317,12 +321,23 @@ def update(self, cp, cp_cam):
     self.can_valid = cp.can_valid
     self.cam_can_valid = cp_cam.can_valid
     msg = None
+    lastspeedlimit = None
+    lastlive_MapData = None
     for socket, event in self.poller.poll(0):
       if socket is self.gps_location:
         msg = messaging.recv_one(socket)
       elif socket is self.lat_Control:
         self.lastlat_Control = messaging.recv_one(socket).latControl
+      elif socket is self.live_MapData:
+        lastlive_MapData =  messaging.recv_one(socket).liveMapData
+    if lastlive_MapData is not None:
+      if lastlive_MapData.speedLimitValid:
+        self.lastspeedlimit = lastlive_MapData.speedLimit
+        self.lastspeedlimitvalid = True
+      else:
+        self.lastspeedlimitvalid = False
 
+
     if msg is not None:
       gps_pkt = msg.gpsLocationExternal
       self.inaccuracy = gps_pkt.accuracy
@@ -505,15 +520,28 @@ def update(self, cp, cp_cam):
       self.generic_toggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM'])
     self.tsgn1 = cp_cam.vl["RSA1"]['TSGN1']
     self.spdval1 = cp_cam.vl["RSA1"]['SPDVAL1']
-    #if self.spdval1 > 0:
-    #  print self.spdval1
+
     self.splsgn1 = cp_cam.vl["RSA1"]['SPLSGN1']
     self.tsgn2 = cp_cam.vl["RSA1"]['TSGN2']
     self.spdval2 = cp_cam.vl["RSA1"]['SPDVAL2']
+
     self.splsgn2 = cp_cam.vl["RSA1"]['SPLSGN2']
     self.tsgn3 = cp_cam.vl["RSA2"]['TSGN3']
     self.splsgn3 = cp_cam.vl["RSA2"]['SPLSGN3']
     self.tsgn4 = cp_cam.vl["RSA2"]['TSGN4']
     self.splsgn4 = cp_cam.vl["RSA2"]['SPLSGN4']
     self.noovertake = self.tsgn1 == 65 or self.tsgn2 == 65 or self.tsgn3 == 65 or self.tsgn4 == 65 or self.tsgn1 == 66 or self.tsgn2 == 66 or self.tsgn3 == 66 or self.tsgn4 == 66
-
+    if self.spdval1 > 0 or self.spdval2 > 0:
+      dat = messaging.new_message()
+      dat.init('liveTrafficData')
+      if self.spdval1 > 0:
+        dat.liveTrafficData.speedLimitValid = True
+        dat.liveTrafficData.speedLimit = self.spdval1
+      else:
+        dat.liveTrafficData.speedLimitValid = False
+      if self.spdval2 > 0:
+        dat.liveTrafficData.speedAdvisoryValid = True
+        dat.liveTrafficData.speedAdvisory = self.spdval2
+      else:
+        dat.liveTrafficData.speedAdvisoryValid = False
+      self.traffic_data_sock.send(dat.to_bytes())

selfdrive/common/version.h

@@ -1 +1 @@
-#define COMMA_VERSION "0.5.10.4-arne182"
+#define COMMA_VERSION "0.5.10.5-arne182"

selfdrive/manager.py

@@ -111,8 +111,16 @@ def unblock_stdout():
 }
 # define process name with niceness factor
 mean_processes = {
-  "controlsd": -20,
-  "boardd": -20,
+  "controlsd": -15,
+  "boardd": -14,
+  "visiond": -13,
+  "plannerd": -12,
+  "loggerd": -11,
+  "radard": -10,
+  "pandad": -9,
+  "locationd": -8,
+  "mapd": -7,
+  "sensord": -6,
 }
 android_packages = ("ai.comma.plus.offroad", "ai.comma.plus.frame")
 

selfdrive/mapd/mapd.py

@@ -131,21 +131,42 @@ def mapsd_thread():
   global last_gps
 
   context = zmq.Context()
+  poller = zmq.Poller()
   gps_sock = messaging.sub_sock(context, service_list['gpsLocation'].port, conflate=True)
-  gps_external_sock = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, conflate=True)
+  gps_external_sock = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, conflate=True, poller=poller)
   map_data_sock = messaging.pub_sock(context, service_list['liveMapData'].port)
+  traffic_data_sock = messaging.sub_sock(context, service_list['liveTrafficData'].port, conflate=True, poller=poller)
 
   cur_way = None
   curvature_valid = False
   curvature = None
   upcoming_curvature = 0.
   dist_to_turn = 0.
   road_points = None
-
+  speedLimittraffic = 0
+  speedLimittraffic_prev = 0
+  max_speed_prev = 0
+  speedLimittrafficvalid = False
+
   while True:
     gps = messaging.recv_one(gps_sock)
-    gps_ext = messaging.recv_one_or_none(gps_external_sock)
-
+    gps_ext = None
+    traffic = None
+    for socket, event in poller.poll(0):
+      if socket is gps_external_sock:
+        gps_ext = messaging.recv_one(socket)
+      elif socket is traffic_data_sock:
+        traffic = messaging.recv_one(socket)
+    if traffic is not None:
+      if traffic.liveTrafficData.speedLimitValid:
+        speedLimittraffic = traffic.liveTrafficData.speedLimit
+      if traffic.liveTrafficData.speedAdvisoryValid:
+        speedLimittrafficAdvisory = traffic.liveTrafficData.speedAdvisory
+        speedLimittrafficAdvisoryvalid = True
+      else:
+        speedLimittrafficAdvisoryvalid = False
+    else:
+      speedLimittrafficAdvisoryvalid = False
     if gps_ext is not None:
       gps = gps_ext.gpsLocationExternal
     else:
@@ -243,8 +264,22 @@ def mapsd_thread():
       # Seed limit
       max_speed = cur_way.max_speed()
       if max_speed is not None:
-        dat.liveMapData.speedLimitValid = True
-        dat.liveMapData.speedLimit = max_speed
+        if max_speed is not max_speed_prev:
+          speedLimittrafficvalid = False
+      if speedLimittraffic_prev is not speedLimittraffic:
+        speedLimittrafficvalid = True
+
+      if speedLimittrafficvalid:
+        if speedLimittraffic is not 0: # Should not occur but check anyway
+          dat.liveMapData.speedLimitValid = True
+          dat.liveMapData.speedLimit = speedLimittraffic / 3.6
+          speedLimittraffic_prev = speedLimittraffic
+      else:
+        if max_speed is not None:
+          dat.liveMapData.speedLimitValid = True
+          dat.liveMapData.speedLimit = max_speed
+          max_speed_prev = max_speed
+
 
         # TODO: use the function below to anticipate upcoming speed limits
         #max_speed_ahead, max_speed_ahead_dist = cur_way.max_speed_ahead(max_speed, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE)
@@ -255,10 +290,14 @@ def mapsd_thread():
 
 
       advisory_max_speed = cur_way.advisory_max_speed()
-      if advisory_max_speed is not None:
+      if speedLimittrafficAdvisoryvalid:
         dat.liveMapData.speedAdvisoryValid = True
-        dat.liveMapData.speedAdvisory = advisory_max_speed
-
+        dat.liveMapData.speedAdvisory = speedLimittrafficAdvisory / 3.6
+      else:
+        if advisory_max_speed is not None:
+          dat.liveMapData.speedAdvisoryValid = True
+          dat.liveMapData.speedAdvisory = advisory_max_speed
+
       # Curvature
       dat.liveMapData.curvatureValid = curvature_valid
       dat.liveMapData.curvature = float(upcoming_curvature)

selfdrive/service_list.yaml

@@ -72,7 +72,8 @@ orbFeaturesSummary: [8062, true]
 driverMonitoring: [8063, true]
 liveParameters: [8064, true]
 liveMapData: [8065, true]
-latControl: [8069, false]
+liveTrafficData: [8079, false]
+latControl: [8078, false]
 cameraOdometry: [8066, true]
 pathPlan: [8067, true]
 kalmanOdometry: [8068, true]