Add extra tags for error logging, add smooth/dynamic gas profile. Tuning soon

op-tools.py

@@ -0,0 +1,55 @@
+import time, sys, json
+from os import system, name
+
+def main_menu():
+    print("Please chooce a category:")
+    while True:
+        print("1. Dynamic follow\n2. GPS\n3. Other")
+        c = input("[1-3]: ")
+        try:
+            int(c)
+        except:
+            return
+        if int(c) - 1 in range(3):
+            break
+    if int(c) == 1:
+        dynamic_follow()
+
+def dynamic_follow():
+    clear()
+    print("\nPress Ctrl + C at any time to quit!")
+    time.sleep(3)
+    try:
+        while True:
+            clear()
+            print("\nDynamic follow data:\n")
+            try:
+                with open("/data/op-tools-data/dynamic-follow.json", "r") as f:
+                    data = json.loads(f.read())
+                    for i in data:
+                        print(i + ": " + str(data[i]))
+            except:
+                print("\nReading error, waiting a second...")
+                time.sleep(1)
+            time.sleep(.15)
+    except KeyboardInterrupt: 
+        print()   
+        main_menu()
+
+def clear(): 
+    # for windows 
+    if name == 'nt': 
+        system('cls') 
+
+    # for mac and linux(here, os.name is 'posix') 
+    else: 
+        system('clear') 
+
+
+main_menu()
+
+
+'''while True:
+    sys.stdout.write("\r\r\r" + time.ctime()+"\n"+time.ctime())
+    sys.stdout.flush()
+    time.sleep(1)'''

panda/board/safety/safety_toyota.h

@@ -16,8 +16,8 @@ const int TOYOTA_MAX_RT_DELTA = 375;      // max delta torque allowed for real t
 const int TOYOTA_RT_INTERVAL = 250000;    // 250ms between real time checks
 
 // longitudinal limits
-const int TOYOTA_MAX_ACCEL = 1800;        // 1.8 m/s2
-const int TOYOTA_MIN_ACCEL = -3600;       // 3.6 m/s2
+const int TOYOTA_MAX_ACCEL = 3500;        // 3.5 m/s2
+const int TOYOTA_MIN_ACCEL = -4000;       // 4.0 m/s2
 
 // global actuation limit state
 int toyota_actuation_limits = 1;          // by default steer limits are imposed

selfdrive/car/toyota/carstate.py

@@ -147,7 +147,7 @@ def __init__(self, CP):
     self.gasMode = int(self.kegman.conf['lastGasMode'])
     self.sloMode = int(self.kegman.conf['lastSloMode'])
     self.sloLabels = ["offset","normal"]
-    self.gasLabels = ["normal","sport","eco"]
+    self.gasLabels = ["smooth","sport","eco"]
     #labelslabels for ALCA modes
     self.alcaLabels = ["MadMax","Normal","Wifey","off"]
     self.alcaMode = int(self.kegman.conf['lastALCAMode'])     # default to last ALCAmode on startup

selfdrive/controls/lib/long_mpc.py

@@ -8,6 +8,7 @@
 from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG
 from scipy import interpolate
 import math
+import json
 
 
 class LongitudinalMpc(object):
@@ -30,6 +31,7 @@ def __init__(self, mpc_id, live_longitudinal_mpc):
     self.mpc_frame = 0  # idea thanks to kegman
     self.relative_velocity = None
     self.relative_distance = None
+    self.dict_builder = {}
 
     try:
       with open("/data/openpilot/gas-interceptor", "r") as f:
@@ -135,6 +137,7 @@ def set_cur_state(self, v, a):
 
   def get_traffic_level(self, lead_vels):  # generate a value to modify TR by based on fluctuations in lead speed
     if len(lead_vels) < 40:
+      self.dict_builder["traffic_modifier"] = None
       return 1.0  # if less than 20 seconds of traffic data do nothing to TR
     lead_vel_diffs = []
     for idx, vel in enumerate(lead_vels):
@@ -143,7 +146,7 @@ def get_traffic_level(self, lead_vels):  # generate a value to modify TR by base
     x = [0, len(lead_vels)]
     y = [1.25, .9]  # min and max values to modify TR by, need to tune
     traffic = interp(sum(lead_vel_diffs), x, y)
-
+    self.dict_builder["traffic_modifier"] = traffic
     return traffic
 
   def get_acceleration(self, velocity_list, is_self):  # calculate acceleration to generate more accurate following distances
@@ -171,20 +174,24 @@ def dynamic_follow(self, velocity):  # in m/s
     TR = interpolate.interp1d(x, y, fill_value='extrapolate')(velocity)[()]  # extrapolate above 90 mph
 
     if self.relative_velocity is not None:
-      x = [-11.2235, -9.4102, -7.1051, -5.7221, -4.3391, -3.5093, -2.7716, -2.2491, -2.0033, -1.6959, -1.0198, 0.0, 0.1788, 0.7935, 1.8692, 2.6683]  # relative velocity values
-      y = [0.423, 0.4037, 0.3566, 0.3266, 0.2859, 0.2623, 0.23, 0.1787, 0.1251, 0.078, 0.036, 0, -0.0613, -0.1384, -0.2413, -0.2991]  # modification values
+      x = [-10.93223, -9.17617, -7.57411, -5.81805, -4.37004, -2.98368, -2.49073, -1.96698, -1.13517, 0.0, 0.12799, 0.77499, 1.85325, 2.6851]  # relative velocity values
+      y = [0.3852, 0.3766, 0.34, 0.3057, 0.2584, 0.1961, 0.1402, 0.0757, 0.0327, 0, -0.0554, -0.1371, -0.2402, -0.3004]  # modification values
       TR = TR + interp(self.relative_velocity, x, y)  # interpolate as to not modify too much
 
       x = [-4.4704, -2.2352, -0.8941, 0.0, 1.3411]   # self acceleration values
       y = [0.158, 0.058, 0.016, 0, -0.13]  # modification values
       TR = TR + interp(self.get_acceleration(self.dynamic_follow_dict["self_vels"], True), x, y)  # factor in self acceleration
+      self.dict_builder["self_accel"] = self.get_acceleration(self.dynamic_follow_dict["self_vels"], True)
 
       x = [-4.4704, -2.2, -0.958, -0.447, -0.2235, 0.0, 0.5588, 1.3411]  # lead acceleration values
       y = [0.3555, 0.2788, 0.165, 0.1, 0.0375, 0, -0.115, -0.195]  # modification values
       TR = TR + interp(self.get_acceleration(self.dynamic_follow_dict["lead_vels"], False), x, y)  # factor in lead car's acceleration; should perform better
+      self.dict_builder["lead_accel"] = self.get_acceleration(self.dynamic_follow_dict["lead_vels"], False)
 
       TR = TR * self.get_traffic_level(self.dynamic_follow_dict["traffic_vels"])  # modify TR based on last minute of traffic data
 
+    self.dict_builder["TR"] = TR
+
     if TR < 0.65:
       return 0.65
     else:
@@ -197,7 +204,10 @@ def generate_cost(self, TR, v_ego):
       real_TR = self.relative_distance / float(v_ego)  # switched to cost generation using actual distance from lead car; should be safer
       if abs(real_TR - TR) >= .25:  # use real TR if diff is greater than x safety threshold
         TR = real_TR
-    return round(float(interp(TR, x, y)), 3)
+
+    cost = round(float(interp(TR, x, y)), 3)
+    self.dict_builder["generated_cost"] = cost
+    return cost
 
   def update(self, CS, lead, v_cruise_setpoint):
     v_ego = CS.carState.vEgo
@@ -242,6 +252,9 @@ def update(self, CS, lead, v_cruise_setpoint):
     # Calculate mpc
     t = sec_since_boot()
     TR = self.calculate_tr(v_ego, CS.carState)
+    '''with open("/data/op-tools-data/dynamic-follow.json", "w") as d_f:
+      d_f.write(json.dumps(self.dict_builder))'''
+    self.dict_builder = {}
     n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, TR)
     duration = int((sec_since_boot() - t) * 1e9)
     self.send_mpc_solution(n_its, duration)

selfdrive/controls/lib/longcontrol.py

@@ -84,10 +84,11 @@ def dynamic_gas(self, v_ego, v_rel, d_rel, gasinterceptor, gasbuttonstatus):
     dynamic = False
     if gasinterceptor:
       if gasbuttonstatus == 0:
-        #dynamic = True
-        #x = [0, 2.2863, 4.5373, 7.3644, 9.0393, 35.0]
-        #y = [0.2, 0.2199, 0.2631, 0.3572, 0.5, 0.7]
-        y = [0.2, 0.5, 0.7]
+        dynamic = True
+        x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703, 24.46614, 29.05805, 32.71015, 35.76326]
+        y = [0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229, 0.34784, 0.36765, 0.38, 0.396, 0.409, 0.425, 0.478, 0.55, 0.621, 0.7]
+        #x = [0.0, 0.6422, 1.36595, 2.25989, 3.22941, 4.06505, 5.64084, 7.00847, 9.2202, 12.96404, 15.42305, 18.11906, 20.11706, 24.46618, 29.0581, 32.7102, 35.76332]  # future
+        #y = [0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229, 0.34784, 0.36765, 0.38, 0.396, 0.409, 0.425, 0.478, 0.55, 0.621, 0.7]
       elif gasbuttonstatus == 1:
         y = [0.25, 0.9, 0.9]
       elif gasbuttonstatus == 2:
@@ -104,16 +105,15 @@ def dynamic_gas(self, v_ego, v_rel, d_rel, gasinterceptor, gasbuttonstatus):
       x = [0., 9., 35.]  # default BP values
 
     accel = interp(v_ego, x, y)
-    if dynamic:  # dynamic gas profile specific operations
-      if v_rel is not None:  # if lead
-        if (v_ego) < 6.7056:  # if under 15 mph
-          x = [0.0, 0.3072, 0.5173, 0.9337, 1.7882]
-          y = [-0.1, -0.0799, -0.0463, -0.0171, 0]
-          accel = accel + interp(v_rel, x, y)
-        else:
-          x = [-0.89408, 0, 0.89408, 4.4704]
-          y = [-.05, 0, .005, .02]
-          accel = accel + interp(v_rel, x, y)
+    if dynamic and v_rel is not None:  # dynamic gas profile specific operations, and if lead
+      if (v_ego) < 6.7056:  # if under 15 mph
+        x = [0.0, 0.3072, 0.5173, 0.9337, 1.7882]
+        y = [-0.125, -0.0799, -0.0463, -0.0171, 0]
+        accel = accel + interp(v_rel, x, y)
+      else:
+        x = [-0.89408, 0, 0.89408, 4.4704]
+        y = [-.05, 0, .005, .02]
+        accel = accel + interp(v_rel, x, y)
 
 
     min_return = 0.05

selfdrive/crash.py

@@ -20,14 +20,8 @@ def install():
 else:
   from raven import Client
   from raven.transport.http import HTTPTransport
-  try:
-    with open("/data/params/d/GitBranch", "r") as f:
-      branch = f.read()
-    f.close()
-  except:
-    branch = "release2"
-
-  error_tags = {'dirty': dirty, 'branch': branch}
+
+  error_tags = {'dirty': dirty, 'branch': 'unknown'}
 
   try:
     with open("/data/data/ai.comma.plus.offroad/files/persistStore/persist-auth", "r") as f:
@@ -39,24 +33,37 @@ def install():
   except:
     pass
 
-  client = Client('https://137e8e621f114f858f4c392c52e18c6d:[email protected]/1404547',
-                  install_sys_hook=False, transport=HTTPTransport, release=version, tags=error_tags)
+  logging_data = {"branch": "/data/params/d/GitBranch", "commit": "/data/params/d/GitCommit", "remote": "/data/params/d/GitRemote"}
+
+  for key in logging_data:
+    try:
+      with open(logging_data[key], "r") as f:
+        error_tags[key] = str(f.read())
+    except:
+      pass
+
+  dsns = ['https://137e8e621f114f858f4c392c52e18c6d:[email protected]/1404547',
+          'https://c58740a8bcc54e3c86dec0cbc8a4ac82:[email protected]/1405746']
+
+  clients = [Client(dsn, install_sys_hook=False, transport=HTTPTransport, release=version, tags=error_tags) for dsn in dsns]
+
 
   def capture_exception(*args, **kwargs):
-    client.captureException(*args, **kwargs)
-    cloudlog.error("crash", exc_info=kwargs.get('exc_info', 1))
+    for client in clients:
+      client.captureException(*args, **kwargs)
+      cloudlog.error("crash", exc_info=kwargs.get('exc_info', 1))
 
   def capture_warning(warning_string):
-    client.captureMessage(warning_string, level='warning')
+    [client.captureMessage(warning_string, level='warning') for client in clients]
 
   def capture_info(info_string):
-    client.captureMessage(info_string, level='info')
+    [client.captureMessage(info_string, level='info') for client in clients]
 
   def bind_user(**kwargs):
-    client.user_context(kwargs)
+    [client.user_context(kwargs) for client in clients]
 
   def bind_extra(**kwargs):
-    client.extra_context(kwargs)
+    [client.extra_context(kwargs) for client in clients]
 
   def install():
     # installs a sys.excepthook

selfdrive/kegman_conf.py

@@ -7,10 +7,14 @@ def __init__(self):
 
   def read_config(self):
     self.element_updated = False
+    self.default_config = {"cameraOffset":"0.06", "lastTrMode":"1", "battChargeMin":"90", "battChargeMax":"95", "wheelTouchSeconds":"1800", "battPercOff":"25", "carVoltageMinEonShutdown":"11200", "brakeStoppingTarget":"0.25", "angle_steers_offset":"0" , "brake_distance_extra":"1" , "lastALCAMode":"1" , "brakefactor":"1.2", "lastGasMode":"0" , "lastSloMode":"1"}
 
     if os.path.isfile('/data/kegman.json'):
       with open('/data/kegman.json', 'r') as f:
-        self.config = json.load(f)
+        try:
+          self.config = json.load(f)
+        except:
+          self.config = self.default_config
       if "battPercOff" not in self.config:
         self.config.update({"battPercOff":"25"})
         self.element_updated = True
@@ -26,16 +30,16 @@ def read_config(self):
       if "brake_distance_extra" not in self.config: # extra braking distance in m
         self.config.update({"brake_distance_extra":"1"})
         self.element_updated = True
-      if "lastALCAMode" not in self.config: # extra braking distance in m
+      if "lastALCAMode" not in self.config:
         self.config.update({"lastALCAMode":"1"})
         self.element_updated = True
       if "brakefactor" not in self.config: # brake at 20% higher speeds than what I like
         self.config.update({"brakefactor":"1.2"}) 
         self.element_updated = True
-      if "lastGasMode" not in self.config: # brake at 20% higher speeds than what I like
+      if "lastGasMode" not in self.config:
         self.config.update({"lastGasMode":"0"}) 
         self.element_updated = True
-      if "lastSloMode" not in self.config: # brake at 20% higher speeds than what I like
+      if "lastSloMode" not in self.config:
         self.config.update({"lastSloMode":"1"}) 
         self.element_updated = True
 
@@ -57,7 +61,7 @@ def read_config(self):
         self.write_config(self.config)
 
     else:
-      self.config = {"cameraOffset":"0.06", "lastTrMode":"1", "battChargeMin":"90", "battChargeMax":"95", "wheelTouchSeconds":"1800", "battPercOff":"25", "carVoltageMinEonShutdown":"11200", "brakeStoppingTarget":"0.25", "angle_steers_offset":"0" , "brake_distance_extra":"1" , "lastALCAMode":"1" , "brakefactor":"1.2", "lastGasMode":"0" , "lastSloMode":"1"}
+      self.config = self.default_config
       self.write_config(self.config)
     return self.config
 

selfdrive/ui/ui.c

@@ -74,15 +74,15 @@ const int UI_FREQ = 60;   // Hz
 const uint8_t bg_colors[][4] = {
   [STATUS_STOPPED] = {0x07, 0x23, 0x39, 0xff},
   [STATUS_DISENGAGED] = {0x17, 0x33, 0x49, 0xff},
-  [STATUS_ENGAGED] = {0x17, 0x86, 0x44, 0xff},
+  [STATUS_ENGAGED] = {0xFF, 0xFF, 0xFF, 0xff},
   [STATUS_WARNING] = {0xDA, 0x6F, 0x25, 0xff},
   [STATUS_ALERT] = {0xC9, 0x22, 0x31, 0xff},
 };
 
 const uint8_t alert_colors[][4] = {
   [STATUS_STOPPED] = {0x07, 0x23, 0x39, 0xf1},
   [STATUS_DISENGAGED] = {0x17, 0x33, 0x49, 0xc8},
-  [STATUS_ENGAGED] = {0x17, 0x86, 0x44, 0xf1},
+  [STATUS_ENGAGED] = {0xFF, 0xFF, 0xFF, 0xf1},
   [STATUS_WARNING] = {0xDA, 0x6F, 0x25, 0xf1},
   [STATUS_ALERT] = {0xC9, 0x22, 0x31, 0xf1},
 };