From 2051d1943d849ddefe34ab0580fc0783cca0be69 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Tue, 23 Apr 2019 19:54:10 +0200
Subject: [PATCH 01/22] add liveTrafficData

---
 selfdrive/service_list.yaml | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/selfdrive/service_list.yaml b/selfdrive/service_list.yaml
index c01c8d743c601a..91e2388fb4128f 100644
--- a/selfdrive/service_list.yaml
+++ b/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]

From 0dcb41e4db157d6f26a920cf25fab6345bcd9316 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Tue, 23 Apr 2019 19:56:59 +0200
Subject: [PATCH 02/22] add LiveTrafficData

---
 cereal/log.capnp | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/cereal/log.capnp b/cereal/log.capnp
index 8f80363dc2082a..7be7185036d895 100644
--- a/cereal/log.capnp
+++ b/cereal/log.capnp
@@ -1639,7 +1639,10 @@ struct LiveMapData {
   distToTurn @10 :Float32;
   mapValid @11 :Bool;
 }
-
+struct LiveTrafficData {
+  speedLimitValid @0 :Bool;
+  speedLimit @1 :Float32;
+}  
 struct LatControl {
   anglelater @0 :Float32;
 }
@@ -1729,5 +1732,6 @@ struct Event {
     cameraOdometry @64 :CameraOdometry;
     pathPlan @65 :PathPlan;
     kalmanOdometry @66 :KalmanOdometry;
+    liveTrafficData @67 :LiveTrafficData;
   }
 }

From 5a2686494503c56a900156a6992880ea6d071501 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Tue, 23 Apr 2019 20:07:38 +0200
Subject: [PATCH 03/22] add speedAdvisory

---
 cereal/log.capnp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/cereal/log.capnp b/cereal/log.capnp
index 7be7185036d895..9ca0897138eb5c 100644
--- a/cereal/log.capnp
+++ b/cereal/log.capnp
@@ -1642,6 +1642,8 @@ struct LiveMapData {
 struct LiveTrafficData {
   speedLimitValid @0 :Bool;
   speedLimit @1 :Float32;
+  speedAdvisoryValid @2 :Bool;
+  speedAdvisory @3 :Float32;
 }  
 struct LatControl {
   anglelater @0 :Float32;

From a92908f30a31a1937363665a3ece7cec5dcaf8fa Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Tue, 23 Apr 2019 20:13:06 +0200
Subject: [PATCH 04/22] Send speed and advisory speed to zmq

---
 selfdrive/car/toyota/carstate.py | 20 +++++++++++++++++---
 1 file changed, 17 insertions(+), 3 deletions(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 7e44340bde46c0..bdb31fe64fdd45 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -206,6 +206,7 @@ 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.traffic_data_sock = messaging.pub_sock(context, service_list['liveTrafficData'].port)
     self.CP = CP
     self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
     self.shifter_values = self.can_define.dv["GEAR_PACKET"]['GEAR']
@@ -502,15 +503,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())

From 4037ac466568248afdd934a36ddf9cf4d669d5d8 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Tue, 23 Apr 2019 20:37:36 +0200
Subject: [PATCH 05/22] send traffic speed limits to mapd

---
 selfdrive/car/toyota/carstate.py | 24 +++++++++++++++++++++---
 1 file changed, 21 insertions(+), 3 deletions(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index bdb31fe64fdd45..1644733538c0ff 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -206,7 +206,11 @@ 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.lastspeedlimit = 0
+    self.lastspeedlimitvalid = False
+    self.spdval1 = 0
     self.CP = CP
     self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
     self.shifter_values = self.can_define.dv["GEAR_PACKET"]['GEAR']
@@ -323,7 +327,15 @@ def update(self, cp, cp_cam):
         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_or_none(socket).latControl
+    if lastlive_MapData is not None:
+      lastspeedlimit = lastlive_MapData.liveMapData.speedLimit
+      if lastspeedlimit is not self.lastspeedlimit:
+        self.lastspeedlimit = lastspeedlimit
+        self.lastspeedlimitvalid = True
+
+      
     if msg is not None:
       gps_pkt = msg.gpsLocationExternal
       self.inaccuracy = gps_pkt.accuracy
@@ -502,7 +514,10 @@ def update(self, cp, cp_cam):
     else:
       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']
+    spdval1 = cp_cam.vl["RSA1"]['SPDVAL1']
+    if spdval1 is not self.spdval1:
+      self.spdval1 = spdval1
+      self.lastspeedlimitvalid = False
     
     self.splsgn1 = cp_cam.vl["RSA1"]['SPLSGN1']
     self.tsgn2 = cp_cam.vl["RSA1"]['TSGN2']
@@ -518,7 +533,10 @@ def update(self, cp, cp_cam):
       dat = messaging.new_message()
       dat.init('liveTrafficData')
       if self.spdval1 > 0:
-        dat.liveTrafficData.speedLimitValid = True
+        if self.lastspeedlimitvalid:
+          dat.liveTrafficData.speedLimitValid = False
+        else:
+          dat.liveTrafficData.speedLimitValid = True
         dat.liveTrafficData.speedLimit = self.spdval1
       else:
         dat.liveTrafficData.speedLimitValid = False

From b3bdd6b77d452595ca4e425cf6e540fbdb5c4f4a Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Tue, 23 Apr 2019 20:48:26 +0200
Subject: [PATCH 06/22] exception if speed limit goes back down to 0

---
 selfdrive/car/toyota/carstate.py | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 1644733538c0ff..9ba5dd31912a1d 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -517,7 +517,10 @@ def update(self, cp, cp_cam):
     spdval1 = cp_cam.vl["RSA1"]['SPDVAL1']
     if spdval1 is not self.spdval1:
       self.spdval1 = spdval1
-      self.lastspeedlimitvalid = False
+      if self.spdval1 = 0:
+        self.lastspeedlimitvalid = True
+      else:
+        self.lastspeedlimitvalid = False
     
     self.splsgn1 = cp_cam.vl["RSA1"]['SPLSGN1']
     self.tsgn2 = cp_cam.vl["RSA1"]['TSGN2']

From ce7296d2d94c271e762dcd338cceb25279d29295 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Tue, 23 Apr 2019 20:49:45 +0200
Subject: [PATCH 07/22] Traffic signs or osm speed limit

If there is a change in traffic signs change then take that value has the speed limit.
If it goes back to not seeing a sign go back to OSM.
If OSM speed changes then ignore signs and use OSM speed limit.
---
 selfdrive/mapd/mapd.py | 32 ++++++++++++++++++++++++++------
 1 file changed, 26 insertions(+), 6 deletions(-)

diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py
index 9f352e1bfd410a..d1767087975244 100755
--- a/selfdrive/mapd/mapd.py
+++ b/selfdrive/mapd/mapd.py
@@ -134,6 +134,7 @@ def mapsd_thread():
   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)
   map_data_sock = messaging.pub_sock(context, service_list['liveMapData'].port)
+  traffic_data_sock = messaging.sub_sock(context, service_list['liveTrafficData'].port, conflate=True)
 
   cur_way = None
   curvature_valid = False
@@ -145,7 +146,18 @@ def mapsd_thread():
   while True:
     gps = messaging.recv_one(gps_sock)
     gps_ext = messaging.recv_one_or_none(gps_external_sock)
-
+    traffic = messaging.recv_one_or_none(traffic_data_sock)
+    if traffic is not None:
+      if traffic.liveTrafficData.speedLimitValid:
+        speedLimittraffic = traffic.liveTrafficData.speedLimit
+        speedLimittrafficvalid = True
+      else:
+        speedLimittrafficvalid = False
+      if traffic.liveTrafficData.speedAdvisoryValid:
+        speedLimittrafficAdvisory = traffic.liveTrafficData.speedAdvisory
+        speedLimittrafficAdvisoryvalid = True
+      else:
+        
     if gps_ext is not None:
       gps = gps_ext.gpsLocationExternal
     else:
@@ -242,9 +254,13 @@ def mapsd_thread():
 
       # Seed limit
       max_speed = cur_way.max_speed()
-      if max_speed is not None:
+      if speedLimittrafficvalid:
         dat.liveMapData.speedLimitValid = True
-        dat.liveMapData.speedLimit = max_speed
+        dat.liveMapData.speedLimit = speedLimittraffic
+      else:
+        if max_speed is not None:
+          dat.liveMapData.speedLimitValid = True
+          dat.liveMapData.speedLimit = 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 +271,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
+      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)

From 5cc1b71226c7656a162a1d863607ca07cb0f761c Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Tue, 23 Apr 2019 20:52:56 +0200
Subject: [PATCH 08/22] Complete logic for if there is no traffic data

---
 selfdrive/mapd/mapd.py | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py
index d1767087975244..f26685869d1eea 100755
--- a/selfdrive/mapd/mapd.py
+++ b/selfdrive/mapd/mapd.py
@@ -157,7 +157,10 @@ def mapsd_thread():
         speedLimittrafficAdvisory = traffic.liveTrafficData.speedAdvisory
         speedLimittrafficAdvisoryvalid = True
       else:
-        
+        speedLimittrafficAdvisoryvalid = False
+    else:
+      speedLimittrafficvalid = False
+      speedLimittrafficAdvisoryvalid = False
     if gps_ext is not None:
       gps = gps_ext.gpsLocationExternal
     else:

From a948d35121b64cf0652675835740c215e78fcac7 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Tue, 23 Apr 2019 21:05:03 +0200
Subject: [PATCH 09/22] fix syntax

---
 selfdrive/car/toyota/carstate.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 9ba5dd31912a1d..07a7a86bb55040 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -517,7 +517,7 @@ def update(self, cp, cp_cam):
     spdval1 = cp_cam.vl["RSA1"]['SPDVAL1']
     if spdval1 is not self.spdval1:
       self.spdval1 = spdval1
-      if self.spdval1 = 0:
+      if self.spdval1 == 0:
         self.lastspeedlimitvalid = True
       else:
         self.lastspeedlimitvalid = False

From 326efde630a8231835e08c85c4f0d558e986739b Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Wed, 24 Apr 2019 09:19:34 +0200
Subject: [PATCH 10/22] Define lastspeedlimit

---
 selfdrive/car/toyota/carstate.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 07a7a86bb55040..152d69cce0f6d9 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -322,6 +322,7 @@ def update(self, cp, cp_cam):
     self.can_valid = cp.can_valid
     self.cam_can_valid = cp_cam.can_valid
     msg = None
+    lastspeedlimit = None
     for socket, event in self.poller.poll(0):
       if socket is self.gps_location:
         msg = messaging.recv_one(socket)

From 2752503957689e8abc8713997a022799c8b9650a Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Wed, 24 Apr 2019 13:41:10 +0200
Subject: [PATCH 11/22] Minor corrections

---
 selfdrive/car/toyota/carstate.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 152d69cce0f6d9..06b742d1b4541e 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -323,13 +323,14 @@ def update(self, cp, cp_cam):
     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_or_none(socket).latControl
+        lastlive_MapData =  messaging.recv_one_or_none(socket).liveMapData
     if lastlive_MapData is not None:
       lastspeedlimit = lastlive_MapData.liveMapData.speedLimit
       if lastspeedlimit is not self.lastspeedlimit:

From 86d3d4bcf53118802428aa3058a3e8e97f6fc6b5 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Wed, 24 Apr 2019 16:08:36 +0200
Subject: [PATCH 12/22] Update carstate.py

---
 selfdrive/car/toyota/carstate.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 06b742d1b4541e..6213352076eda1 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -332,7 +332,7 @@ def update(self, cp, cp_cam):
       elif socket is self.live_MapData:
         lastlive_MapData =  messaging.recv_one_or_none(socket).liveMapData
     if lastlive_MapData is not None:
-      lastspeedlimit = lastlive_MapData.liveMapData.speedLimit
+      lastspeedlimit = lastlive_MapData.speedLimit
       if lastspeedlimit is not self.lastspeedlimit:
         self.lastspeedlimit = lastspeedlimit
         self.lastspeedlimitvalid = True

From 3c7c95c1ab1d1333f131d0769b7532d56ec0feb3 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Thu, 25 Apr 2019 10:08:09 +0200
Subject: [PATCH 13/22] Minor correction

---
 selfdrive/car/toyota/carstate.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 6213352076eda1..e3956cd4c9393a 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -330,7 +330,7 @@ def update(self, cp, cp_cam):
       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_or_none(socket).liveMapData
+        lastlive_MapData =  messaging.recv_one(socket).liveMapData
     if lastlive_MapData is not None:
       lastspeedlimit = lastlive_MapData.speedLimit
       if lastspeedlimit is not self.lastspeedlimit:

From 19b0bd1460f460cbaa21ba8ed865a3c4f1dbcdcb Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Thu, 25 Apr 2019 10:31:02 +0200
Subject: [PATCH 14/22] Add poller

---
 selfdrive/mapd/mapd.py | 14 ++++++++++----
 1 file changed, 10 insertions(+), 4 deletions(-)

diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py
index f26685869d1eea..46c2ed83d7d31c 100755
--- a/selfdrive/mapd/mapd.py
+++ b/selfdrive/mapd/mapd.py
@@ -131,10 +131,11 @@ 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)
+  traffic_data_sock = messaging.sub_sock(context, service_list['liveTrafficData'].port, conflate=True, poller=poller)
 
   cur_way = None
   curvature_valid = False
@@ -145,8 +146,13 @@ def mapsd_thread():
 
   while True:
     gps = messaging.recv_one(gps_sock)
-    gps_ext = messaging.recv_one_or_none(gps_external_sock)
-    traffic = messaging.recv_one_or_none(traffic_data_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

From d691e39bff41d5edd39d73656527636bdfcbc993 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Thu, 25 Apr 2019 12:46:20 +0200
Subject: [PATCH 15/22] Fix syntax

---
 selfdrive/mapd/mapd.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py
index 46c2ed83d7d31c..e2632bce1cc048 100755
--- a/selfdrive/mapd/mapd.py
+++ b/selfdrive/mapd/mapd.py
@@ -151,7 +151,7 @@ def mapsd_thread():
     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
+      elif socket is traffic_data_sock:
         traffic = messaging.recv_one(socket)
     if traffic is not None:
       if traffic.liveTrafficData.speedLimitValid:

From a0bc5ebc23a12eb0192b9603223ad89c1683e66c Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Thu, 25 Apr 2019 14:06:28 +0200
Subject: [PATCH 16/22] Move logic to mapd

---
 selfdrive/car/toyota/carstate.py | 23 +++++++----------------
 1 file changed, 7 insertions(+), 16 deletions(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index e3956cd4c9393a..eeb6768a92e997 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -208,8 +208,7 @@ def __init__(self, CP):
     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.lastspeedlimit = 0
-    self.lastspeedlimitvalid = False
+    
     self.spdval1 = 0
     self.CP = CP
     self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
@@ -332,10 +331,11 @@ def update(self, cp, cp_cam):
       elif socket is self.live_MapData:
         lastlive_MapData =  messaging.recv_one(socket).liveMapData
     if lastlive_MapData is not None:
-      lastspeedlimit = lastlive_MapData.speedLimit
-      if lastspeedlimit is not self.lastspeedlimit:
-        self.lastspeedlimit = lastspeedlimit
+      if lastlive_MapData.speedLimitValid:
+        self.lastspeedlimit = lastlive_MapData.speedLimit
         self.lastspeedlimitvalid = True
+      else:
+        self.lastspeedlimitvalid = False
 
       
     if msg is not None:
@@ -516,13 +516,7 @@ def update(self, cp, cp_cam):
     else:
       self.generic_toggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM'])
     self.tsgn1 = cp_cam.vl["RSA1"]['TSGN1']
-    spdval1 = cp_cam.vl["RSA1"]['SPDVAL1']
-    if spdval1 is not self.spdval1:
-      self.spdval1 = spdval1
-      if self.spdval1 == 0:
-        self.lastspeedlimitvalid = True
-      else:
-        self.lastspeedlimitvalid = False
+    self.spdval1 = cp_cam.vl["RSA1"]['SPDVAL1']
     
     self.splsgn1 = cp_cam.vl["RSA1"]['SPLSGN1']
     self.tsgn2 = cp_cam.vl["RSA1"]['TSGN2']
@@ -538,10 +532,7 @@ def update(self, cp, cp_cam):
       dat = messaging.new_message()
       dat.init('liveTrafficData')
       if self.spdval1 > 0:
-        if self.lastspeedlimitvalid:
-          dat.liveTrafficData.speedLimitValid = False
-        else:
-          dat.liveTrafficData.speedLimitValid = True
+        dat.liveTrafficData.speedLimitValid = True
         dat.liveTrafficData.speedLimit = self.spdval1
       else:
         dat.liveTrafficData.speedLimitValid = False

From 67b7a8e93fa2a11cb5b518035b9fe534d61886ea Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Thu, 25 Apr 2019 17:34:59 +0200
Subject: [PATCH 17/22] include logic in mapd to switch between traffic signs
 and osm

---
 selfdrive/mapd/mapd.py | 24 ++++++++++++++++--------
 1 file changed, 16 insertions(+), 8 deletions(-)

diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py
index e2632bce1cc048..ca6257fa6541a1 100755
--- a/selfdrive/mapd/mapd.py
+++ b/selfdrive/mapd/mapd.py
@@ -143,7 +143,10 @@ def mapsd_thread():
   upcoming_curvature = 0.
   dist_to_turn = 0.
   road_points = None
-
+  speedLimittraffic_prev = 0
+  max_speed_prev = 0
+  speedLimittrafficvalid = False
+  
   while True:
     gps = messaging.recv_one(gps_sock)
     gps_ext = None
@@ -156,16 +159,12 @@ def mapsd_thread():
     if traffic is not None:
       if traffic.liveTrafficData.speedLimitValid:
         speedLimittraffic = traffic.liveTrafficData.speedLimit
-        speedLimittrafficvalid = True
-      else:
-        speedLimittrafficvalid = False
       if traffic.liveTrafficData.speedAdvisoryValid:
         speedLimittrafficAdvisory = traffic.liveTrafficData.speedAdvisory
         speedLimittrafficAdvisoryvalid = True
       else:
         speedLimittrafficAdvisoryvalid = False
     else:
-      speedLimittrafficvalid = False
       speedLimittrafficAdvisoryvalid = False
     if gps_ext is not None:
       gps = gps_ext.gpsLocationExternal
@@ -263,13 +262,22 @@ def mapsd_thread():
 
       # Seed limit
       max_speed = cur_way.max_speed()
+      if max_speed is not None and max_speed is not max_speed_prev:
+        speedLimittrafficvalid = False
+      if speedLimittraffic_prev is not speedLimittraffic:
+        speedLimittrafficvalid = True
+      
       if speedLimittrafficvalid:
-        dat.liveMapData.speedLimitValid = True
-        dat.liveMapData.speedLimit = speedLimittraffic
+        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)
@@ -282,7 +290,7 @@ def mapsd_thread():
       advisory_max_speed = cur_way.advisory_max_speed()
       if speedLimittrafficAdvisoryvalid:
         dat.liveMapData.speedAdvisoryValid = True
-        dat.liveMapData.speedAdvisory = speedLimittrafficAdvisory
+        dat.liveMapData.speedAdvisory = speedLimittrafficAdvisory / 3.6
       else:
         if advisory_max_speed is not None:
           dat.liveMapData.speedAdvisoryValid = True

From 4a08bb644ee2a9f73830bf514dc330d6a84db757 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Thu, 25 Apr 2019 18:09:54 +0200
Subject: [PATCH 18/22] Seperate if statememts

---
 selfdrive/mapd/mapd.py | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py
index ca6257fa6541a1..fb8420005a1179 100755
--- a/selfdrive/mapd/mapd.py
+++ b/selfdrive/mapd/mapd.py
@@ -262,8 +262,9 @@ def mapsd_thread():
 
       # Seed limit
       max_speed = cur_way.max_speed()
-      if max_speed is not None and max_speed is not max_speed_prev:
-        speedLimittrafficvalid = False
+      if max_speed is not None:
+        if max_speed is not max_speed_prev:
+          speedLimittrafficvalid = False
       if speedLimittraffic_prev is not speedLimittraffic:
         speedLimittrafficvalid = True
       

From 5d4e71005d382aeb937fbd37dbec445b005873d9 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Thu, 25 Apr 2019 18:32:58 +0200
Subject: [PATCH 19/22] Fix referenced before assignment

---
 selfdrive/mapd/mapd.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py
index fb8420005a1179..39fc060a34af85 100755
--- a/selfdrive/mapd/mapd.py
+++ b/selfdrive/mapd/mapd.py
@@ -143,6 +143,7 @@ def mapsd_thread():
   upcoming_curvature = 0.
   dist_to_turn = 0.
   road_points = None
+  speedLimittraffic = 0
   speedLimittraffic_prev = 0
   max_speed_prev = 0
   speedLimittrafficvalid = False

From 56d1c9caa9b894da7d981b7a842e099104f8a396 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Thu, 25 Apr 2019 21:34:19 +0200
Subject: [PATCH 20/22] attempt to fix @praeluceo problem

---
 selfdrive/manager.py | 14 +++++++++++---
 1 file changed, 11 insertions(+), 3 deletions(-)

diff --git a/selfdrive/manager.py b/selfdrive/manager.py
index 2a5035f76a9bf7..8592c107791a8a 100755
--- a/selfdrive/manager.py
+++ b/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")
 
@@ -205,7 +213,7 @@ def start_managed_process(name):
 
   if name in mean_processes:
     try:
-      subprocess.call(["renice", "-n", str(mean_processes[name]), str(running[name].pid)])
+      subprocess.call(["sudo renice", "-n", str(mean_processes[name]), str(running[name].pid)])
     except:
       cloudlog.warning("failed to renice process %s" % name)
 

From 59f41fbfde27f8f1ccabb4c68793fc49ae34c398 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Thu, 25 Apr 2019 21:39:48 +0200
Subject: [PATCH 21/22] sudo not available on eon

---
 selfdrive/manager.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/manager.py b/selfdrive/manager.py
index 8592c107791a8a..1f649b669265fe 100755
--- a/selfdrive/manager.py
+++ b/selfdrive/manager.py
@@ -213,7 +213,7 @@ def start_managed_process(name):
 
   if name in mean_processes:
     try:
-      subprocess.call(["sudo renice", "-n", str(mean_processes[name]), str(running[name].pid)])
+      subprocess.call(["renice", "-n", str(mean_processes[name]), str(running[name].pid)])
     except:
       cloudlog.warning("failed to renice process %s" % name)
 

From 0eb44fae09cb0abe4dbef6304d357a0100fe48f5 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Thu, 25 Apr 2019 22:10:12 +0200
Subject: [PATCH 22/22] Update version.h

---
 selfdrive/common/version.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h
index a20c9ec32f8727..51b9a55ec780b1 100644
--- a/selfdrive/common/version.h
+++ b/selfdrive/common/version.h
@@ -1 +1 @@
-#define COMMA_VERSION "0.5.10.4-arne182"
+#define COMMA_VERSION "0.5.10.5-arne182"