From cef90290319ee90c2a5e98292427ae2c32954720 Mon Sep 17 00:00:00 2001 From: Comma Device Date: Wed, 25 Mar 2020 23:46:00 -0400 Subject: [PATCH] First ALPHA candidate! --- cereal/messaging/__init__.py | 59 +++++++++++++++++++ selfdrive/car/tesla/interface.py | 1 + selfdrive/car/tesla/radar_interface.py | 10 ++-- .../car/tesla/radar_tools/calibrateRadar.py | 2 +- selfdrive/controls/controlsd.py | 7 +-- selfdrive/controls/radard.py | 1 + selfdrive/ui/ui.cc | 6 +- 7 files changed, 72 insertions(+), 14 deletions(-) diff --git a/cereal/messaging/__init__.py b/cereal/messaging/__init__.py index 085c05af9aeddd..01de59f7eccfc7 100644 --- a/cereal/messaging/__init__.py +++ b/cereal/messaging/__init__.py @@ -142,6 +142,7 @@ def __init__(self, services, ignore_alive=None, addr="127.0.0.1"): self.data = {} self.logMonoTime = {} self.valid = {} + self.valid_cnt = {s: 0 for s in services} if ignore_alive is not None: self.ignore_alive = ignore_alive @@ -162,6 +163,10 @@ def __init__(self, services, ignore_alive=None, addr="127.0.0.1"): self.data[s] = getattr(data, s) self.logMonoTime[s] = 0 self.valid[s] = data.valid + if data.valid: + self.valid_cnt[s] = 0 + else: + self.valid_cnt[s] +=1 def __getitem__(self, s): return self.data[s] @@ -211,6 +216,60 @@ def all_alive_and_valid(self, service_list=None): service_list = self.alive.keys() return self.all_alive(service_list=service_list) and self.all_valid(service_list=service_list) + def all_alive_with_info(self, service_list=None): + """Returns alive state for tracked processes. + Args: + service_list (list): Optional service list. + Returns: + tuple: areAllAlive, processName, count + """ + if service_list is None: # check all + service_list = self.alive.keys() + areAllAlive = True + processName = "" + count = 0 + for s in service_list: + if not self.alive[s]: + areAllAlive = False + processName = s + count = self.valid_cnt[s] + break + return (areAllAlive, processName, count) + + def all_valid_with_info(self, service_list=None): + """Returns valid state for tracked processes. + Args: + service_list (list): Optional service list. + Returns: + tuple: areAllValid, processName, count + """ + if service_list is None: # check all + service_list = self.valid.keys() + areAllValid = True + processName = "" + count = 0 + for s in service_list: + if not self.valid[s]: + areAllValid = False + processName = s + count = self.valid_cnt[s] + break + return (areAllValid, processName, count) + + def all_alive_and_valid_with_info(self, service_list=None): + """Returns alive and valid state for tracked processes. + Args: + service_list (list): Optional service list. + Returns: + tuple: areAllAlive, areAllValid, aliveProcessName, aliveCount, validProcessName, validCount + """ + if service_list is None: # check all + service_list = self.alive.keys() + areAllAlive, aliveProcessName, aliveCount = self.all_alive(service_list=service_list) + areAllValid, validProcessName, validCount = self.all_valid(service_list=service_list) + return (areAllAlive, areAllValid, aliveProcessName, aliveCount, validProcessName, ) + + class PubMaster(): def __init__(self, services): diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 22829a8b2c87bd..a3812b524d6514 100644 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -222,6 +222,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.steerRateCost = 1.0 ret.radarOffCan = not CarSettings().get_value("useTeslaRadar") + ret.radarTimeStep = 0.05 #20Hz return ret diff --git a/selfdrive/car/tesla/radar_interface.py b/selfdrive/car/tesla/radar_interface.py index 75398fb2d3fdc4..096f75278164e5 100644 --- a/selfdrive/car/tesla/radar_interface.py +++ b/selfdrive/car/tesla/radar_interface.py @@ -2,13 +2,14 @@ import os import time from cereal import car, tesla -from selfdrive.can.parser import CANParser -from common.realtime import DT_RDR +from opendbc.can.parser import CANParser from cereal.services import service_list import cereal.messaging as messaging from selfdrive.car.interfaces import RadarInterfaceBase from selfdrive.car.tesla.readconfig import CarSettings from selfdrive.tinklad.tinkla_interface import TinklaClient +from selfdrive.car.interfaces import RadarInterfaceBase + BOSCH_MAX_DIST = 250. #max distance for radar #use these for tracks (5 tracks) @@ -67,11 +68,10 @@ class RadarInterface(RadarInterfaceBase): tinklaClient = TinklaClient() def __init__(self,CP): - super().__init__(self) # radar self.pts = {} self.extPts = {} - self.delay = int(0.1 / DT_RDR) + self.delay = 0 self.useTeslaRadar = CarSettings().get_value("useTeslaRadar") self.TRACK_LEFT_LANE = True self.TRACK_RIGHT_LANE = True @@ -87,7 +87,7 @@ def __init__(self,CP): self.trackId = 1 self.trigger_start_msg = RADAR_A_MSGS[0] self.trigger_end_msg = RADAR_B_MSGS[-1] - + self.radar_ts = CP.radarTimeStep def update(self, can_strings,v_ego): diff --git a/selfdrive/car/tesla/radar_tools/calibrateRadar.py b/selfdrive/car/tesla/radar_tools/calibrateRadar.py index 884f2a9804f8d1..a98c551ee286b8 100644 --- a/selfdrive/car/tesla/radar_tools/calibrateRadar.py +++ b/selfdrive/car/tesla/radar_tools/calibrateRadar.py @@ -8,7 +8,7 @@ from cereal import car import time import os -from selfdrive.can.parser import CANParser +from opendbc.can.parser import CANParser from common.realtime import sec_since_boot from cereal.services import service_list import cereal.messaging as messaging diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 06c65ae14a7619..64fc7a91e2ada1 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -232,11 +232,8 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr # check if user has interacted with the car driver_engaged = len(CS.buttonEvents) > 0 or \ v_cruise_kph != v_cruise_kph_last or \ - CS.steeringPressed or \ - not dm_enabled + CS.steeringPressed - # add eventual driver distracted events - events = driver_status.update(events, driver_engaged, isActive(state), CS.standstill) if CS.leftBlinker or CS.rightBlinker: last_blinker_frame = frame @@ -540,8 +537,6 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): prof = Profiler(False) # off by default - dm_enabled = CarSettings().get_value("enableDriverMonitor") - while True: start_time = sec_since_boot() prof.checkpoint("Ratekeeper", ignore=True) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index c5e5e8a2437f0b..1251c4d46dd87d 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -100,6 +100,7 @@ class RadarD(): def __init__(self, radar_ts, mocked, RI,use_tesla_radar, delay=0): self.current_time = 0 self.mocked = mocked + self.RI = RI self.tracks = defaultdict(dict) self.kalman_params = KalmanParams(radar_ts) diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 193cf62f1230b7..c4ab5e9674981a 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -1011,7 +1011,6 @@ int main(int argc, char* argv[]) { s->scene.satelliteCount = -1; s->scene.uilayout_sidebarcollapsed = false; ui_draw(s); - bb_ui_draw_UI(s) ; glFinish(); should_swap = true; } @@ -1034,7 +1033,10 @@ int main(int argc, char* argv[]) { // Don't waste resources on drawing in case screen is off if (s->awake) { ui_draw(s); - bb_ui_draw_UI(s) ; + if (s->vision_connected) { + bb_ui_draw_UI(s) ; + ui_draw_infobar(s); + } glFinish(); should_swap = true; }