stay up to date

common/op_params.py

@@ -80,6 +80,7 @@ def __init__(self):
                         'cloak': Param(True, bool, "make comma believe you are on their fork"),
                         #'corolla_tss2_d_tuning': Param(False, bool, 'lateral tuning using PID w/ true derivative'),
                         'default_brake_distance': Param(250.0, VT.number, 'Distance in m to start braking for mapped speeds.'),
+                        'distance_traveled': Param(False, bool, 'Whether to log distance_traveled or not.'),
                         #'enable_long_derivative': Param(False, bool, 'If you have longitudinal overshooting, enable this! This enables derivative-based\n'
                                                                     # 'integral wind-down to help reduce overshooting within the long PID loop'),
                         #'dynamic_follow': Param('normal', str, "Can be: ('close', 'normal', 'far'): Left to right increases in following distance.\n"

common/params_pyx.pyx

@@ -27,6 +27,9 @@ keys = {
   b"CompletedTrainingVersion": [TxType.PERSISTENT],
   b"DisablePowerDown": [TxType.PERSISTENT],
   b"DisableUpdates": [TxType.PERSISTENT],
+  b"DistanceTraveled": [TxType.PERSISTENT],
+  b"DistanceTraveledEngaged": [TxType.PERSISTENT],
+  b"DistanceTraveledOverride": [TxType.PERSISTENT],
   b"DoUninstall": [TxType.CLEAR_ON_MANAGER_START],
   b"DongleId": [TxType.PERSISTENT],
   b"GitBranch": [TxType.PERSISTENT],

selfdrive/car/toyota/carcontroller.py

@@ -342,7 +342,7 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
         can_sends.append(poll_blindspot_status(RIGHT_BLINDSPOT))
         #print("debug Right blindspot poll")
 
-    if frame > 200:
+    if frame > 200 and CS.CP.carFingerprint not in TSS2_CAR:
       if frame % 100 == 0:
         if speed_signs_in_mph:
           smartspeed =round(CS.smartspeed*2.23694)

selfdrive/car/toyota/interface.py

@@ -39,7 +39,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
     ret.steerActuatorDelay = 0.12  # Default delay, Prius has larger delay
     ret.steerLimitTimer = 0.4
 
-    if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H, CAR.PRIUS_TSS2]:  # These cars use LQR/INDI
+    if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H]:  # These cars use LQR/INDI
       ret.lateralTuning.init('pid')
       ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]]
       ret.lateralTuning.pid.kdBP, ret.lateralTuning.pid.kdV = [[0.], [0.]]
@@ -82,7 +82,6 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       tire_stiffness_factor = 0.6371   # hand-tune
       ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG
       ret.steerActuatorDelay = 0.5
-      #ret.steerLimitTimer = 0.70
       if prius_pid:
         ret.lateralTuning.init('pid')
         ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]]
@@ -112,6 +111,8 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
         ret.lateralTuning.indi.actuatorEffectivenessBP = [16.7, 25, 36.1]
         ret.lateralTuning.indi.actuatorEffectivenessV = [9.5, 15, 15]
 
+
+
     elif candidate in [CAR.RAV4, CAR.RAV4H]:
       stop_and_go = True if (candidate in CAR.RAV4H) else False
       ret.safetyParam = 73
@@ -122,25 +123,27 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       if ret.enableGasInterceptor:
         ret.longitudinalTuning.kpV = [0.4, 0.36, 0.325]  # arne's tune.
         ret.longitudinalTuning.kiV = [0.195, 0.10]
-      #ret.lateralTuning.init('lqr')
-      #ret.lateralTuning.lqr.scale = 1500.0
-      #ret.lateralTuning.lqr.ki = 0.05
-      #ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
-      #ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
-      #ret.lateralTuning.lqr.c = [1., 0.]
-      #ret.lateralTuning.lqr.k = [-110.73572306, 451.22718255]
-      #ret.lateralTuning.lqr.l = [0.3233671, 0.3185757]
-      #ret.lateralTuning.lqr.dcGain = 0.002237852961363602
-      ret.lateralTuning.init('indi')
-      ret.lateralTuning.indi.innerLoopGainBP = [18, 22, 26]
-      ret.lateralTuning.indi.innerLoopGainV = [9, 12, 15]
-      ret.lateralTuning.indi.outerLoopGainBP = [18, 22, 26]
-      ret.lateralTuning.indi.outerLoopGainV = [8, 11, 14.99]
-      ret.lateralTuning.indi.timeConstantBP = [18, 22, 26]
-      ret.lateralTuning.indi.timeConstantV = [1, 3, 4.5]
-      ret.lateralTuning.indi.actuatorEffectivenessBP = [18, 22, 26]
-      ret.lateralTuning.indi.actuatorEffectivenessV = [9, 12, 15]
-      ret.steerActuatorDelay = 0.42
+      if spairrowtuning:
+        ret.lateralTuning.init('indi')
+        ret.lateralTuning.indi.innerLoopGainBP = [18, 22, 26]
+        ret.lateralTuning.indi.innerLoopGainV = [9, 12, 15]
+        ret.lateralTuning.indi.outerLoopGainBP = [18, 22, 26]
+        ret.lateralTuning.indi.outerLoopGainV = [8, 11, 14.99]
+        ret.lateralTuning.indi.timeConstantBP = [18, 22, 26]
+        ret.lateralTuning.indi.timeConstantV = [1, 3, 4.5]
+        ret.lateralTuning.indi.actuatorEffectivenessBP = [18, 22, 26]
+        ret.lateralTuning.indi.actuatorEffectivenessV = [9, 12, 15]
+        ret.steerActuatorDelay = 0.4
+      else:
+        ret.lateralTuning.init('lqr')
+        ret.lateralTuning.lqr.scale = 1500.0
+        ret.lateralTuning.lqr.ki = 0.05
+        ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
+        ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
+        ret.lateralTuning.lqr.c = [1., 0.]
+        ret.lateralTuning.lqr.k = [-110.73572306, 451.22718255]
+        ret.lateralTuning.lqr.l = [0.3233671, 0.3185757]
+        ret.lateralTuning.lqr.dcGain = 0.002237852961363602
 
     elif candidate == CAR.COROLLA:
       stop_and_go = False
@@ -276,16 +279,16 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.longitudinalTuning.kpV = [1.3, 1.0, 0.7]
       ret.longitudinalTuning.kiBP = [0., 5., 12., 20., 27.]
       ret.longitudinalTuning.kiV = [.35, .23, .20, .17, .1]
-      ret.stoppingBrakeRate = 0.165 # reach stopping target smoothly
+      ret.stoppingBrakeRate = 0.16 # reach stopping target smoothly
       ret.startingBrakeRate = 0.9 # release brakes fast
       ret.startAccel = 1.50 # Accelerate from 0 faster
       ret.steerActuatorDelay = 0
-      ret.steerLimitTimer = 0.4
+      ret.steerLimitTimer = 0.1
       ret.lateralTuning.init('indi')
       ret.lateralTuning.indi.innerLoopGainBP = [16.7, 25]
       ret.lateralTuning.indi.innerLoopGainV = [15, 15]
-      ret.lateralTuning.indi.outerLoopGainBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2,  25, 30.6, 33.3, 36.1]
-      ret.lateralTuning.indi.outerLoopGainV = [4.65, 6.45, 8.25, 10.05, 11.85, 13.65, 14.99, 14.99, 14.99, 14.99]
+      ret.lateralTuning.indi.outerLoopGainBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 30.6, 33.3, 36.1]
+      ret.lateralTuning.indi.outerLoopGainV = [4.55, 6.35, 8.15, 9.95, 11.75, 13.55, 14.99, 14.99, 14.99, 14.99]
       ret.lateralTuning.indi.timeConstantBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 30.6, 33.3, 36.1]
       ret.lateralTuning.indi.timeConstantV = [1.0, 1.3, 1.6, 1.9, 2.2, 2.5, 2.8, 3.4, 3.7, 4.0]
       ret.lateralTuning.indi.actuatorEffectivenessBP = [16.7, 25]
@@ -323,7 +326,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.steerRatio = 15.33
       tire_stiffness_factor = 0.996  # not optimized yet
       ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG
-      ret.steerActuatorDelay = 0.48
+      ret.steerActuatorDelay = 0.52
       ret.steerLimitTimer = 5.0
       if spairrowtuning:
         ret.lateralTuning.init('indi')
@@ -339,9 +342,9 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       else:
         ret.lateralTuning.pid.kpBP = [0.0]
         ret.lateralTuning.pid.kiBP = [0.0]
-        ret.lateralTuning.pid.kpV = [0.028]
+        ret.lateralTuning.pid.kpV = [0.036]
         ret.lateralTuning.pid.kiV = [0.0012]
-        ret.lateralTuning.pid.kf = 0.000153263811757641 # hardcoded in latcontrol_pid, this does nothing for now
+        ret.lateralTuning.pid.kf = 0.000153263811757641
         ret.lateralTuning.pid.newKfTuned = True
 
     elif candidate in [CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2]:

selfdrive/car/toyota/toyotacan.py

@@ -36,7 +36,7 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, distan
     "DISTANCE": distance,
     "MINI_CAR": lead,
     "SET_ME_X3": 3,
-    "PERMIT_BRAKING": lead,
+    "PERMIT_BRAKING": 1,
     "RELEASE_STANDSTILL": not standstill_req,
     "CANCEL_REQ": pcm_cancel,
   }

selfdrive/controls/controlsd.py

@@ -21,9 +21,13 @@
 from selfdrive.controls.lib.vehicle_model import VehicleModel
 from selfdrive.controls.lib.planner import LON_MPC_STEP
 from selfdrive.locationd.calibrationd import Calibration
-#from common.travis_checker import travis
+from common.travis_checker import travis
 #import threading
 from selfdrive.interceptor import Interceptor
+from common.op_params import opParams
+op_params = opParams()
+
+distance_traveled = op_params.get('distance_traveled')
 
 LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
 LANE_DEPARTURE_THRESHOLD = 0.1
@@ -135,7 +139,19 @@ def __init__(self, sm=None, pm=None, can_sock=None):
     self.can_error_counter = 0
     self.last_blinker_frame = 0
     self.saturated_count = 0
-    self.distance_traveled = 0
+    self.distance_traveled_now = 0
+    self.distance_traveled_now = op_params.get('distance_traveled')
+    if not travis:
+      self.distance_traveled = float(params.get("DistanceTraveled", encoding='utf8'))
+      self.distance_traveled_engaged = float(params.get("DistanceTraveledEngaged", encoding='utf8'))
+      self.distance_traveled_override = float(params.get("DistanceTraveledOverride", encoding='utf8'))
+    else:
+      self.distance_traveled = 0
+      self.distance_traveled_engaged = 0
+      self.distance_traveled_override = 0
+
+    self.distance_traveled_frame = 0
+
     self.last_functional_fan_frame = 0
     self.events_prev = []
     self.current_alert_types = [ET.PERMANENT]
@@ -247,7 +263,7 @@ def update_events(self, CS):
     if not self.sm['liveLocationKalman'].sensorsOK and not NOSENSOR:
       if self.sm.frame > 5 / DT_CTRL:  # Give locationd some time to receive all the inputs
         self.events.add(EventName.sensorDataInvalid)
-    if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000):
+    if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled_now > 1000):
       # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes
       if not (SIMULATION or NOSENSOR):  # TODO: send GPS in carla
         self.events.add(EventName.noGps)
@@ -340,7 +356,17 @@ def data_sample(self):
     if not self.sm['dragonConf'].dpAtl and not self.sm['health'].controlsAllowed and self.enabled:
       self.mismatch_counter += 1
 
+    self.distance_traveled_now += CS.vEgo * DT_CTRL
     self.distance_traveled += CS.vEgo * DT_CTRL
+    if self.enabled:
+      self.distance_traveled_engaged += CS.vEgo * DT_CTRL
+      if CS.steeringPressed:
+        self.distance_traveled_override += CS.vEgo * DT_CTRL
+    if (self.sm.frame - self.distance_traveled_frame) * DT_CTRL > 10.0 and not travis:
+      put_nonblocking("DistanceTraveled", str(round(self.distance_traveled,2)))
+      put_nonblocking("DistanceTraveledEngaged", str(round(self.distance_traveled_engaged,2)))
+      put_nonblocking("DistanceTraveledOverride", str(round(self.distance_traveled_override,2)))
+      self.distance_traveled_frame = self.sm.frame
 
     return CS
 
@@ -358,7 +384,7 @@ def state_transition(self, CS):
       #print("there")
       self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
       #print(" v_cruise_kph = " + str(self.v_cruise_kph))
-    
+
 
     # decrease the soft disable timer at every step, as it's reset on
     # entrance in SOFT_DISABLING state
@@ -471,8 +497,8 @@ def state_control(self, CS):
       if (lac_log.saturated and not CS.steeringPressed) or \
               (self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):
         # Check if we deviated from the path
-        left_deviation = actuators.steer > 0 and path_plan.dPoly[3] > 0.1
-        right_deviation = actuators.steer < 0 and path_plan.dPoly[3] < -0.1
+        left_deviation = actuators.steer > 0 and path_plan.dPoly[3] > 0.15
+        right_deviation = actuators.steer < 0 and path_plan.dPoly[3] < -0.15
 
         if left_deviation or right_deviation:
           self.events.add(EventName.steerSaturated)

selfdrive/controls/lib/pathplanner.py

@@ -14,11 +14,21 @@
 LaneChangeState = log.PathPlan.LaneChangeState
 LaneChangeDirection = log.PathPlan.LaneChangeDirection
 
-LOG_MPC = os.environ.get('LOG_MPC', False)
+LOG_MPC = os.environ.get('LOG_MPC', True)
 
 LANE_CHANGE_SPEED_MIN = 45 * CV.MPH_TO_MS
 LANE_CHANGE_TIME_MAX = 10.
 
+# dp
+
+DP_OFF = 0
+
+DP_ECO = 1
+
+DP_NORMAL = 2
+
+DP_SPORT = 3
+
 DESIRES = {
   LaneChangeDirection.none: {
     LaneChangeState.off: log.PathPlan.Desire.none,
@@ -91,7 +101,7 @@ def update(self, sm, pm, CP, VM):
     v_ego = sm['carState'].vEgo
     angle_steers = sm['carState'].steeringAngle
     active = sm['controlsState'].active
-
+    dp_profile = sm['dragonConf'].dpAccelProfile
     angle_offset = sm['liveParameters'].angleOffset
 
     # Run MPC
@@ -149,7 +159,7 @@ def update(self, sm, pm, CP, VM):
           # we only set timer when in preLaneChange state, dragon_auto_lc_delay delay
           if self.lane_change_state == LaneChangeState.preLaneChange:
             self.dragon_auto_lc_timer = cur_time + sm['dragonConf'].dpAutoLcDelay
-        elif cur_time >= self.dragon_auto_lc_timer:
+        elif cur_time >= (self.dragon_auto_lc_timer - dp_profile + 1):
           # if timer is up, we set torque_applied to True to fake user input
           torque_applied = True
           self.dp_did_auto_lc = True

selfdrive/crash.py

@@ -82,11 +82,25 @@ def install():
     dongle_id = params.get("DongleId").decode('utf8')
   except AttributeError:
     dongle_id = "None"
+  try:
+    distance_traveled = params.get("DistanceTraveled").decode('utf8')
+  except AttributeError:
+    distance_traveled = "None"
+  try:
+    distance_traveled_engaged = params.get("DistanceTraveledEngaged").decode('utf8')
+  except AttributeError:
+    distance_traveled_engaged = "None"
+  try:
+    distance_traveled_override = params.get("DistanceTraveledOverride").decode('utf8')
+  except AttributeError:
+    distance_traveled_override = "None"
   try:
     ip = requests.get('https://checkip.amazonaws.com/').text.strip()
   except Exception:
     ip = "255.255.255.255"
-  error_tags = {'dirty': dirty, 'dongle_id': dongle_id, 'branch': branch, 'remote': origin}
+  error_tags = {'dirty': dirty, 'dongle_id': dongle_id, 'branch': branch, 'remote': origin, 
+                'distance_traveled': distance_traveled, 'distance_traveled_engaged': distance_traveled_engaged, 
+                'distance_traveled_override': distance_traveled_override}
   #uniqueID = op_params.get('uniqueID')
   username = opParams().get('username')
   if username is None or not isinstance(username, str):

selfdrive/manager.py

@@ -204,6 +204,7 @@ def unblock_stdout():
   "calibrationd": "selfdrive.locationd.calibrationd",
   "paramsd": "selfdrive.locationd.paramsd",
   "camerad": ("selfdrive/camerad", ["./camerad"]),
+  "trafficd": ("selfdrive/trafficd", ["./trafficd"]),
   "sensord": ("selfdrive/sensord", ["./sensord"]),
   "clocksd": ("selfdrive/clocksd", ["./clocksd"]),
   "gpsd": ("selfdrive/sensord", ["./gpsd"]),
@@ -286,6 +287,7 @@ def get_running():
 if traffic_lights:
   car_started_processes += [
     'traffic_manager',
+    'trafficd',
   ]
 
 if WEBCAM:
@@ -631,6 +633,9 @@ def main():
     ("OpenpilotEnabledToggle", "1"),
     ("LaneChangeEnabled", "1"),
     ("IsDriverViewEnabled", "0"),
+    ("DistanceTraveled", "0"),
+    ("DistanceTraveledEngaged", "0"),
+    ("DistanceTraveledOverride", "0"),
   ]
 
   # set unset params