diff --git a/cereal/log.capnp b/cereal/log.capnp index 8f80363dc2082a..9ca0897138eb5c 100644 --- a/cereal/log.capnp +++ b/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; } } diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index f3737a0882108d..cafe347eb14a7e 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/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()) 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" diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 2a5035f76a9bf7..1f649b669265fe 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") diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py index 9f352e1bfd410a..39fc060a34af85 100755 --- a/selfdrive/mapd/mapd.py +++ b/selfdrive/mapd/mapd.py @@ -131,9 +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, poller=poller) cur_way = None curvature_valid = False @@ -141,11 +143,30 @@ def mapsd_thread(): 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) 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]