From 2c9c7eca145fbcf240b487ae9ba8dceb15f8d787 Mon Sep 17 00:00:00 2001 From: Shane Date: Thu, 23 May 2019 19:48:53 -0500 Subject: [PATCH 01/12] test --- selfdrive/kegman_conf.py | 1 - 1 file changed, 1 deletion(-) diff --git a/selfdrive/kegman_conf.py b/selfdrive/kegman_conf.py index b8a90d5e1f8abe..754aae373360bf 100644 --- a/selfdrive/kegman_conf.py +++ b/selfdrive/kegman_conf.py @@ -101,7 +101,6 @@ def write_config(conf): # never to be called outside kegman_conf json.dump(conf, f, indent=2, sort_keys=True) os.chmod(kegman_file, 0o764) - def save(data): # allows for writing multiple key/value pairs global conf global thread_counter From 079969ef085938d37315800f6b55eb992786e6e1 Mon Sep 17 00:00:00 2001 From: Shane Date: Thu, 23 May 2019 20:04:09 -0500 Subject: [PATCH 02/12] reduce TR_mod from 6 to 0 mph --- selfdrive/controls/lib/long_mpc.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index 831fdadfc8656a..e7eb8322f97034 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -162,7 +162,7 @@ def get_traffic_level(self, lead_vels): # generate a value to modify TR by base x = [0, len(lead_vels)] y = [1.35, 1.0] # min and max values to modify TR by traffic = interp(sum(lead_vel_diffs), x, y) - return traffic + return float(traffic) def get_acceleration(self, velocity_list, is_self): # calculate acceleration to generate more accurate following distances a = 0.0 @@ -201,7 +201,7 @@ def dynamic_follow(self, velocity): # in m/s if self.stop_and_go: # this allows a smooth deceleration to a stop, while being able to have smooth stop and go x = [stop_and_go_magic_number / 2.0, stop_and_go_magic_number] # from 10 to 20 mph, ramp 1.8 sng distance to regular dynamic follow value - y = [1.8, interp(x[1], x_vel, y_mod)] + y = [1.8, interp(stop_and_go_magic_number, x_vel, y_mod)] TR = interp(velocity, x, y) else: TR = interpolate.interp1d(x_vel, y_mod, fill_value='extrapolate')(velocity)[()] # extrapolate above 90 mph @@ -223,12 +223,12 @@ def dynamic_follow(self, velocity): # in m/s y = [0.95, 1.06, 1.1215, 1.1845, 1.2568, 1.313, 1.34] TR_mod *= interp(self.relative_distance, x, y) # factor in distance from lead car to try and brake quicker - x = [0, 2.2352, 22.352, 33.528] # 0, 5, 50, 75 mph - y = [.25, 1.0, 1.0, .90] # multiply sum of all TR modifications by this - TR += (float(TR_mod) * interp(velocity, x, y)) # lower TR modification for stop and go, and at higher speeds - - TR = float(TR) * self.get_traffic_level(self.dynamic_follow_dict["traffic_vels"]) # modify TR based on last minute of traffic data + x = [0, 2.68224, 22.352, 33.528] # 0, 5, 50, 75 mph + y = [.2, 1.0, 1.0, .90] # multiply sum of all TR modifications by this + TR_mod *= float(interp(velocity, x, y)) # lower TR modification for stop and go, and at higher speeds + TR += TR_mod + TR *= self.get_traffic_level(self.dynamic_follow_dict["traffic_vels"]) # modify TR based on last minute of traffic data if TR < 0.65: return 0.65 else: From bde9a71fd490af71ae488abcce459b62cc05047d Mon Sep 17 00:00:00 2001 From: Shane Date: Thu, 23 May 2019 20:11:40 -0500 Subject: [PATCH 03/12] speed modification tuning --- selfdrive/controls/lib/long_mpc.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index e7eb8322f97034..c95837bb36e0c1 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -223,8 +223,8 @@ def dynamic_follow(self, velocity): # in m/s y = [0.95, 1.06, 1.1215, 1.1845, 1.2568, 1.313, 1.34] TR_mod *= interp(self.relative_distance, x, y) # factor in distance from lead car to try and brake quicker - x = [0, 2.68224, 22.352, 33.528] # 0, 5, 50, 75 mph - y = [.2, 1.0, 1.0, .90] # multiply sum of all TR modifications by this + x = [1.34112, 4.95, 7.6, 22.352, 33.528] + y = [.4, .92, 1.0, 1.0, .90] TR_mod *= float(interp(velocity, x, y)) # lower TR modification for stop and go, and at higher speeds TR += TR_mod From 365c094ac168df419e9589fb85ad460e79a71c9a Mon Sep 17 00:00:00 2001 From: Shane Date: Sat, 25 May 2019 07:17:46 -0500 Subject: [PATCH 04/12] test --- selfdrive/manager.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index c60fd06abd7cbc..ff0e65b6d1d336 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -329,7 +329,7 @@ def manager_thread(): # now loop context = zmq.Context() thermal_sock = messaging.sub_sock(context, service_list['thermal'].port) - gps_sock = messaging.sub_sock(context, service_list['gpsLocation'].port, conflate=True) + #gps_sock = messaging.sub_sock(context, service_list['gpsLocation'].port, conflate=True) cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) @@ -353,11 +353,11 @@ def manager_thread(): while 1: # get health of board, log this in "thermal" msg = messaging.recv_sock(thermal_sock, wait=True) - gps = messaging.recv_one(gps_sock) + '''gps = messaging.recv_one(gps_sock) if 47.3024876979 < gps.gpsLocation.latitude and 54.983104153 > gps.gpsLocation.latitude and gps.gpsLocation.longitude > 5.98865807458 and gps.gpsLocation.longitude < 15.0169958839: logger_dead = True else: - logger_dead = False + logger_dead = False''' # uploader is gated based on the phone temperature if msg.thermal.thermalStatus >= ThermalStatus.yellow: kill_managed_process("uploader") From 1eff0cfad8c59e5108c0137c20a0811f9b113cdd Mon Sep 17 00:00:00 2001 From: Shane Date: Sat, 25 May 2019 07:21:16 -0500 Subject: [PATCH 05/12] test --- selfdrive/ui/ui.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index 40f5f17271c44f..a16d2e73ee8e47 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -2391,7 +2391,7 @@ int main() { } } else { // Car started, fetch a new rgb image from ipc and peek for zmq events. - touched = touch_poll(&touch, &touch_x, &touch_y, s->awake ? 0 : 200); + touched = touch_poll(&touch, &touch_x, &touch_y, s->awake ? 0 : 2); //touched = touch_read(&touch, &touch_x, &touch_y); ui_update(s); if(!s->vision_connected) { From a4fee8438ee83a9eed32a070d8b16276753f9577 Mon Sep 17 00:00:00 2001 From: Shane Date: Sat, 25 May 2019 07:25:01 -0500 Subject: [PATCH 06/12] revert --- selfdrive/ui/ui.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index a16d2e73ee8e47..40f5f17271c44f 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -2391,7 +2391,7 @@ int main() { } } else { // Car started, fetch a new rgb image from ipc and peek for zmq events. - touched = touch_poll(&touch, &touch_x, &touch_y, s->awake ? 0 : 2); + touched = touch_poll(&touch, &touch_x, &touch_y, s->awake ? 0 : 200); //touched = touch_read(&touch, &touch_x, &touch_y); ui_update(s); if(!s->vision_connected) { From 1e45b1f9de3e5ee36c445c8cfbcbd463deb0a116 Mon Sep 17 00:00:00 2001 From: Shane Date: Sat, 25 May 2019 07:43:26 -0500 Subject: [PATCH 07/12] should fix cruise fault errors and openpilot hanging on start up --- selfdrive/manager.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index ff0e65b6d1d336..f79e4d33550b1a 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -329,7 +329,7 @@ def manager_thread(): # now loop context = zmq.Context() thermal_sock = messaging.sub_sock(context, service_list['thermal'].port) - #gps_sock = messaging.sub_sock(context, service_list['gpsLocation'].port, conflate=True) + gps_sock = messaging.sub_sock(context, service_list['gpsLocation'].port, conflate=True) cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) @@ -353,11 +353,12 @@ def manager_thread(): while 1: # get health of board, log this in "thermal" msg = messaging.recv_sock(thermal_sock, wait=True) - '''gps = messaging.recv_one(gps_sock) - if 47.3024876979 < gps.gpsLocation.latitude and 54.983104153 > gps.gpsLocation.latitude and gps.gpsLocation.longitude > 5.98865807458 and gps.gpsLocation.longitude < 15.0169958839: - logger_dead = True - else: - logger_dead = False''' + gps = messaging.recv_one(gps_sock) + if gps: + if 47.3024876979 < gps.gpsLocation.latitude < 54.983104153 and 5.98865807458 < gps.gpsLocation.longitude < 15.0169958839: + logger_dead = True + else: + logger_dead = False # uploader is gated based on the phone temperature if msg.thermal.thermalStatus >= ThermalStatus.yellow: kill_managed_process("uploader") From 1273ee1feebdec353ac46171df00528802a1c583 Mon Sep 17 00:00:00 2001 From: Shane Date: Sat, 25 May 2019 08:04:55 -0500 Subject: [PATCH 08/12] redundancy --- selfdrive/manager.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index f79e4d33550b1a..317a3ba4623203 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -354,7 +354,7 @@ def manager_thread(): # get health of board, log this in "thermal" msg = messaging.recv_sock(thermal_sock, wait=True) gps = messaging.recv_one(gps_sock) - if gps: + if gps and gps.gpsLocation.latitude and gps.gpsLocation.longitude: if 47.3024876979 < gps.gpsLocation.latitude < 54.983104153 and 5.98865807458 < gps.gpsLocation.longitude < 15.0169958839: logger_dead = True else: From f91e7d090d38ea9e19caf5cd43eb6c49af2e8033 Mon Sep 17 00:00:00 2001 From: Shane Date: Sat, 25 May 2019 08:07:49 -0500 Subject: [PATCH 09/12] possibly running too frequently --- selfdrive/manager.py | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 317a3ba4623203..8756eff12dbe0c 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -349,16 +349,14 @@ def manager_thread(): params = Params() logger_dead = False + gps = messaging.recv_one(gps_sock) + if gps and gps.gpsLocation.latitude and gps.gpsLocation.longitude: + if 47.3024876979 < gps.gpsLocation.latitude < 54.983104153 and 5.98865807458 < gps.gpsLocation.longitude < 15.0169958839: + logger_dead = True while 1: # get health of board, log this in "thermal" msg = messaging.recv_sock(thermal_sock, wait=True) - gps = messaging.recv_one(gps_sock) - if gps and gps.gpsLocation.latitude and gps.gpsLocation.longitude: - if 47.3024876979 < gps.gpsLocation.latitude < 54.983104153 and 5.98865807458 < gps.gpsLocation.longitude < 15.0169958839: - logger_dead = True - else: - logger_dead = False # uploader is gated based on the phone temperature if msg.thermal.thermalStatus >= ThermalStatus.yellow: kill_managed_process("uploader") From acea19af4e21d1f4716f3e54aad2c495bd8e25e0 Mon Sep 17 00:00:00 2001 From: Shane Date: Sat, 25 May 2019 08:10:51 -0500 Subject: [PATCH 10/12] test --- selfdrive/manager.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 8756eff12dbe0c..5e098fff99b579 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -349,10 +349,10 @@ def manager_thread(): params = Params() logger_dead = False - gps = messaging.recv_one(gps_sock) + '''gps = messaging.recv_one(gps_sock) if gps and gps.gpsLocation.latitude and gps.gpsLocation.longitude: if 47.3024876979 < gps.gpsLocation.latitude < 54.983104153 and 5.98865807458 < gps.gpsLocation.longitude < 15.0169958839: - logger_dead = True + logger_dead = True''' while 1: # get health of board, log this in "thermal" From 276067737a63e86f4e02c7ddfc572f69813554d7 Mon Sep 17 00:00:00 2001 From: Shane Date: Sat, 25 May 2019 08:22:21 -0500 Subject: [PATCH 11/12] possible fix --- selfdrive/manager.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 5e098fff99b579..24ede8debcd00b 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -349,14 +349,16 @@ def manager_thread(): params = Params() logger_dead = False - '''gps = messaging.recv_one(gps_sock) - if gps and gps.gpsLocation.latitude and gps.gpsLocation.longitude: - if 47.3024876979 < gps.gpsLocation.latitude < 54.983104153 and 5.98865807458 < gps.gpsLocation.longitude < 15.0169958839: - logger_dead = True''' while 1: # get health of board, log this in "thermal" msg = messaging.recv_sock(thermal_sock, wait=True) + gps = messaging.recv_one_or_none(gps_sock) + if gps: + if 47.3024876979 < gps.gpsLocation.latitude < 54.983104153 and 5.98865807458 < gps.gpsLocation.longitude < 15.0169958839: + logger_dead = True + else: + logger_dead = False # uploader is gated based on the phone temperature if msg.thermal.thermalStatus >= ThermalStatus.yellow: kill_managed_process("uploader") From 84f670eceb6da75005984fec235b3e6b8be9299e Mon Sep 17 00:00:00 2001 From: Shane Date: Sat, 25 May 2019 14:48:26 -0500 Subject: [PATCH 12/12] dynamic follow tuning. to-do: add separate long acceleration mod --- selfdrive/controls/lib/long_mpc.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index c95837bb36e0c1..2b9a3701ca51d0 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -219,12 +219,12 @@ def dynamic_follow(self, velocity): # in m/s y = [0.37909, 0.30045, 0.20378, 0.04158, 0, -0.115, -0.195] # modification values TR_mod += interp(self.get_acceleration(self.dynamic_follow_dict["lead_vels"], False), x, y) # factor in lead car's acceleration; should perform better - x = [0, 37.6085, 50.3843, 54.6429, 65.3908, 83.0336, 93.1731] # distance in meters - y = [0.95, 1.06, 1.1215, 1.1845, 1.2568, 1.313, 1.34] + x = [0, 6.9128, 16.0047, 27.163, 37.6085, 50.3843, 54.6429, 65.3908, 83.0336, 93.1731] # distance in meters + y = [1.0175, 1.0079, 1.0045, 1.0083, 1.0176, 1.0547, 1.0911, 1.1454, 1.1838, 1.195] TR_mod *= interp(self.relative_distance, x, y) # factor in distance from lead car to try and brake quicker - x = [1.34112, 4.95, 7.6, 22.352, 33.528] - y = [.4, .92, 1.0, 1.0, .90] + x = [1.1594, 2.7298, 6.1562, 10.5105, 22.352, 33.528] # speed in m/s + y = [0.7, 0.885, 1.0, 1.024, 1.0, 0.9] TR_mod *= float(interp(velocity, x, y)) # lower TR modification for stop and go, and at higher speeds TR += TR_mod