Phantom

selfdrive/car/toyota/radar_interface.py

@@ -9,7 +9,7 @@
 import selfdrive.messaging as messaging
 from selfdrive.car.toyota.values import NO_DSU_CAR
 
-
+phantom = True
 RADAR_A_MSGS = list(range(0x210, 0x220))
 RADAR_B_MSGS = list(range(0x220, 0x230))
 
@@ -81,20 +81,34 @@ def update(self):
         # print ii, self.valid_cnt[ii], score, cpt['VALID'], cpt['LONG_DIST'], cpt['LAT_DIST']
 
         # radar point only valid if it's a valid measurement and score is above 50
-        if cpt['VALID'] or (score > 50 and cpt['LONG_DIST'] < 255 and self.valid_cnt[ii] > 0):
+        if not phantom:
+          if cpt['VALID'] or (score > 50 and cpt['LONG_DIST'] < 255 and self.valid_cnt[ii] > 0):
+            if ii not in self.pts or cpt['NEW_TRACK']:
+              self.pts[ii] = car.RadarState.RadarPoint.new_message()
+              self.pts[ii].trackId = self.track_id
+              self.track_id += 1
+
+            self.pts[ii].dRel = cpt['LONG_DIST']  # from front of car
+            self.pts[ii].yRel = -cpt['LAT_DIST']  # in car frame's y axis, left is positive
+            self.pts[ii].vRel = cpt['REL_SPEED']
+            self.pts[ii].aRel = float('nan')
+            self.pts[ii].yvRel = float('nan')
+            self.pts[ii].measured = bool(cpt['VALID'])
+          else:
+            if ii in self.pts:
+              del self.pts[ii]
+        else:
           if ii not in self.pts or cpt['NEW_TRACK']:
             self.pts[ii] = car.RadarState.RadarPoint.new_message()
             self.pts[ii].trackId = self.track_id
             self.track_id += 1
-          self.pts[ii].dRel = cpt['LONG_DIST']  # from front of car
+
+          self.pts[ii].dRel = 4.0  # from front of car
           self.pts[ii].yRel = -cpt['LAT_DIST']  # in car frame's y axis, left is positive
-          self.pts[ii].vRel = cpt['REL_SPEED']
+          self.pts[ii].vRel = 0.0
           self.pts[ii].aRel = float('nan')
           self.pts[ii].yvRel = float('nan')
           self.pts[ii].measured = bool(cpt['VALID'])
-        else:
-          if ii in self.pts:
-            del self.pts[ii]
 
     ret.points = self.pts.values()
     return ret

selfdrive/controls/lib/planner.py

@@ -148,14 +148,9 @@ def update(self, CS, CP, VM, PP, live20, live100, md, live_map_data):
       if socket is self.lat_Control:
         self.lastlat_Control = messaging.recv_one(socket).latControl
 
-    self.lead_1 = live20.live20.leadOne
-    self.lead_2 = live20.live20.leadTwo
-
-
     lead_1 = live20.live20.leadOne
     lead_2 = live20.live20.leadTwo
 
-
     enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
     following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
 

selfdrive/controls/radard.py

@@ -18,6 +18,9 @@
 from common.params import Params
 from common.realtime import set_realtime_priority, Ratekeeper
 from common.kalman.ekf import EKF, SimpleSensor
+import selfdrive.kegman_conf as kegman
+
+phantom = True
 
 DEBUG = False
 
@@ -110,11 +113,17 @@ def radard_thread(gctx=None):
 
   rk = Ratekeeper(rate, print_delay_threshold=np.inf)
   while 1:
+    kegman.start_thread(5)  # keeps the read thread alive
     rr = RI.update()
 
     ar_pts = {}
-    for pt in rr.points:
-      ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured]
+    if phantom:
+      tmp_dRel = kegman.get("dRel")
+      for pt in rr.points:
+        ar_pts[pt.trackId] = [(tmp_dRel if tmp_dRel is not None else 4.0) + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured]
+    else:
+      for pt in rr.points:
+        ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured]
 
     # receive the live100s
     l100 = None

selfdrive/kegman_conf.py

@@ -6,6 +6,13 @@
 from selfdrive.swaglog import cloudlog
 from common.basedir import BASEDIR
 
+thread_counter = 0  # don't change
+thread_timeout = 5.0  # minutes to wait before stopping thread. reading or writing will reset the counter
+thread_interval = 30.0  # seconds to sleep between checks
+thread_started = False
+kegman_file = "/data/kegman.json"
+variables_written = []
+
 def read_config():
   default_config = {"cameraOffset": "0.06", "lastTrMode": "1", "battChargeMin": "90", "battChargeMax": "95",
                     "wheelTouchSeconds": "1800", "battPercOff": "25", "carVoltageMinEonShutdown": "11200",
@@ -54,7 +61,9 @@ def read_config():
     config = default_config
   return config
 
-def kegman_thread():  # read and write thread; now merges changes from file and variable
+conf = read_config()
+
+def kegman_thread(t_i=thread_interval):  # read and write thread; now merges changes from file and variable
   global conf
   global thread_counter
   global variables_written
@@ -63,7 +72,7 @@ def kegman_thread():  # read and write thread; now merges changes from file and
   try:
     while True:
       thread_counter += 1
-      time.sleep(thread_interval)  # every n seconds check for conf change
+      time.sleep(t_i)  # every n seconds check for conf change
       with open(kegman_file, "r") as f:
         conf_tmp = json.load(f)
       if conf != last_conf or conf != conf_tmp:  # if either variable or file has changed
@@ -113,9 +122,9 @@ def save(data):  # allows for writing multiple key/value pairs
     variables_written.append(key)
   conf.update(data)
 
-def get(key_s=""):  # can get multiple keys from a list
+def get(key_s=None):  # can get multiple keys from a list
   global thread_counter
-  if key_s == "":  # get all
+  if key_s == None:  # get all
     return conf
   else:
     thread_counter = 0
@@ -126,10 +135,9 @@ def get(key_s=""):  # can get multiple keys from a list
     else:
       return None
 
-thread_counter = 0  # don't change
-thread_timeout = 5.0  # minutes to wait before stopping thread. reading or writing will reset the counter
-thread_interval = 30.0  # seconds to sleep between checks
-thread_started = False
-kegman_file = "/data/kegman.json"
-variables_written = []
-conf = read_config()
+def start_thread(t_i=thread_interval):  # call if file needs to read live updates from kegman.json but not write
+  global thread_started
+  if not thread_started and BASEDIR == "/data/openpilot":
+    threading.Thread(target=kegman_thread, args=(t_i,)).start()  # automatically start write thread if file needs it
+    thread_started = True
+    print("Starting thread!")