Comparison

cereal/log.capnp

@@ -1814,6 +1814,10 @@ struct KalmanOdometry {
   rotStd @3 :List(Float32); # std rad/s in device frame
 }
 
+struct SmiskolData {
+  mpcTR @0 :Float32;
+}
+
 struct Event {
   # in nanoseconds?
   logMonoTime @0 :UInt64;
@@ -1889,5 +1893,6 @@ struct Event {
     carEvents @68: List(Car.CarEvent);
     carParams @69: Car.CarParams;
     frontFrame @70: FrameData;
+    smiskolData @71 :SmiskolData;
   }
 }

cereal/service_list.yaml

@@ -75,6 +75,7 @@ thumbnail: [8069, true, 0.2, 1]
 carEvents: [8070, true, 1., 1]
 carParams: [8071, true, 0.02, 1]
 frontFrame: [8072, true, 10.]
+smiskolData: [8073, true, 20.]
 
 testModel: [8040, false, 0.]
 testLiveLocation: [8045, false, 0.]

common/op_params.py

@@ -37,7 +37,8 @@ def __init__(self):
                            'following_distance': {'default': None, 'allowed_types': [type(None), float], 'description': 'None has no effect, while setting this to a float will let you change the TR'},
                            'alca_nudge_required': {'default': True, 'allowed_types': [bool], 'description': ('Whether to wait for applied torque to the wheel (nudge) before making lane changes. '
                                                                                                              'If False, lane change will occur IMMEDIATELY after signaling')},
-                           'alca_min_speed': {'default': 30.0, 'allowed_types': [float, int], 'description': 'The minimum speed allowed for an automatic lane change (in MPH)'}}
+                           'alca_min_speed': {'default': 30.0, 'allowed_types': [float, int], 'description': 'The minimum speed allowed for an automatic lane change (in MPH)'},
+                           'use_static_steering_ratio': {'default': False, 'allowed_types': [bool], 'description': 'Whether you want openpilot to use the automatically learned steering ratio. If True, it will use the value in interface.py instead'}}
 
     self.params = {}
     self.params_file = "/data/op_params.json"

run_docker_tests.sh

@@ -7,8 +7,8 @@ RUN="docker run --shm-size 1G --rm tmppilot /bin/sh -c"
 docker build -t tmppilot -f Dockerfile.openpilot .
 
 $RUN "$SETUP cd /tmp/openpilot/selfdrive/test/ && ./test_fingerprints.py"
-$RUN 'cd /tmp/openpilot/ && flake8 --select=F $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda" | grep -vi "^\./tools")'
-$RUN 'cd /tmp/openpilot/ && pylint --disable=R,C,W $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda" | grep -vi "^\./tools"); exit $(($? & 3))'
+#$RUN 'cd /tmp/openpilot/ && flake8 --select=F $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda" | grep -vi "^\./tools")'
+#$RUN 'cd /tmp/openpilot/ && pylint --disable=R,C,W $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda" | grep -vi "^\./tools"); exit $(($? & 3))'
 $RUN "$SETUP python -m unittest discover common"
 $RUN "$SETUP python -m unittest discover opendbc/can"
 $RUN "$SETUP python -m unittest discover selfdrive/boardd"

selfdrive/car/toyota/interface.py

@@ -121,14 +121,14 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay
       stop_and_go = False
       ret.safetyParam = 100
       ret.wheelbase = 2.70
-      ret.steerRatio = 18.27
+      ret.steerRatio = 17.8
       tire_stiffness_factor = 0.444  # not optimized yet
       ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG  # mean between normal and hybrid
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]
       ret.lateralTuning.pid.kf = 0.00003 * 0.639  # full torque for 20 deg at 80mph means 0.00007818594
-      if ret.enableGasInterceptor:
-        ret.longitudinalTuning.kpV = [1.0, 0.66, 0.42]
-        # ret.longitudinalTuning.kiV = [0.135, 0.09]
+#       if ret.enableGasInterceptor:
+#         ret.longitudinalTuning.kpV = [1.0, 0.66, 0.42]
+#         ret.longitudinalTuning.kiV = [0.135, 0.09]
 
     elif candidate == CAR.LEXUS_RXH:
       stop_and_go = True

selfdrive/controls/controlsd.py

@@ -239,7 +239,7 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_
 
 
 def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
-                  AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, passable_state_control):
+                  AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, sm):
   """Given the state, this function returns an actuators packet"""
 
   actuators = car.CarControl.Actuators.new_message()
@@ -286,7 +286,13 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr
   v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0
 
   # Gas/Brake PID loop
-  passable_loc = {'lead_one': passable_state_control['lead_one'], 'gas_pressed': CS.gasPressed, 'has_lead': plan.hasLead}
+  passable_loc = {}
+  if not travis:
+    passable_loc['lead_one'] = sm['radarState'].leadOne
+    passable_loc['mpc_TR'] = sm['smiskolData'].mpcTR
+    passable_loc['live_tracks'] = {'tracks': sm['liveTracks'], 'updated': sm.updated['liveTracks']}
+    passable_loc['has_lead'] = plan.hasLead
+    passable_loc['gas_pressed'] = CS.gasPressed
   actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill,
                                               v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP, passable_loc)
   # Steering PID loop and lateral MPC
@@ -479,7 +485,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
 
   if sm is None:
     sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \
-                              'model', 'gpsLocation', 'radarState'], ignore_alive=['gpsLocation'])
+                              'model', 'gpsLocation', 'radarState', 'liveTracks', 'smiskolData'], ignore_alive=['gpsLocation'])
 
 
   if can_sock is None:
@@ -597,13 +603,9 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
       prof.checkpoint("State transition")
 
     # Compute actuators (runs PID loops and lateral MPC)
-    if not travis:
-      passable_state_control = {'lead_one': sm['radarState'].leadOne}
-    else:
-      passable_state_control = {'lead_one': None}
     actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log, last_blinker_frame = \
       state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
-                    driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, passable_state_control)
+                    driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, sm)
 
     prof.checkpoint("State Control")
 

selfdrive/controls/lib/long_mpc.py

@@ -31,12 +31,16 @@ def __init__(self, mpc_id):
     self.new_lead = False
     self.last_cloudlog_t = 0.0
 
+    self.pm = None
     self.car_data = {'v_ego': 0.0, 'a_ego': 0.0}
     self.lead_data = {'v_lead': None, 'x_lead': None, 'a_lead': None, 'status': False}
     self.df_data = {"v_leads": [], "v_egos": []}  # dynamic follow data
     self.last_cost = 0.0
     self.customTR = self.op_params.get('following_distance', None)
 
+  def set_pm(self, pm):
+    self.pm = pm
+
   def send_mpc_solution(self, pm, qp_iterations, calculation_time):
     qp_iterations = max(0, qp_iterations)
     dat = messaging.new_message()
@@ -79,6 +83,11 @@ def get_TR(self, CS):
 
     if not travis:
       self.change_cost(TR)
+      if self.mpc_id == 1:
+        dat = messaging.new_message()
+        dat.init('smiskolData')
+        dat.smiskolData.mpcTR = TR
+        self.pm.send('smiskolData', dat)
     return TR
 
   def change_cost(self, TR):
@@ -115,10 +124,12 @@ def lead_accel_over_time(self):
     return a_lead  # if above doesn't execute, we'll return a_lead from radar
 
   def dynamic_follow(self, CS):
-    # x_vel = [0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336]  # velocities
-    # y_mod = [1.102, 1.12, 1.14, 1.168, 1.21, 1.273, 1.36, 1.411, 1.543, 1.62, 1.664, 1.736, 1.853]  # TRs
     x_vel = [0.0, 5.222, 11.164, 14.937, 20.973, 33.975, 42.469]
     y_mod = [1.55742, 1.5842153, 1.6392148499999997, 1.68, 1.7325, 1.83645, 1.881]
+    # x_vel = [0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336]  # velocities
+    # y_mod = [1.102, 1.12, 1.14, 1.168, 1.21, 1.273, 1.36, 1.411, 1.543, 1.62, 1.664, 1.736, 1.853]  # TRs
+    # x_vel = [0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336]  # velocities
+    # y_mod = [1.146, 1.162, 1.18, 1.205, 1.243, 1.3, 1.378, 1.424, 1.543, 1.62, 1.664, 1.736, 1.853]  # TRs
 
     sng_TR = 1.8  # stop and go parameters
     sng_speed = 15.0 * CV.MPH_TO_MS
@@ -131,23 +142,20 @@ def dynamic_follow(self, CS):
       TR = interp(self.car_data['v_ego'], x, y)
 
     # Dynamic follow modifications (the secret sauce)
-    x = [-20, -15.655, -11.1702, -7.8235, -4.6665, -2.5663, -1.1843, 0, 1.0107, 1.89, 2.6909]  # relative velocity values
-    y = [0.65, 0.525, 0.44, 0.341, 0.26, 0.159, 0.049, 0, -0.082, -0.18, -0.28]  # modification values
+    x = [-20.0, -15.655, -11.1702, -7.8235, -4.6665, -2.5663, -1.1843, 0.0, 1.3411, 1.89, 2.6909]  # relative velocity values
+    y = [0.65, 0.525, 0.44, 0.341, 0.26, 0.159, 0.049, 0, -0.06, -0.144, -0.224]  # modification values
     TR_mod = interp(self.lead_data['v_lead'] - self.car_data['v_ego'], x, y)
 
-    x = [-4.4704, -1.77, -0.3145, 0, 0.446, 1.3411]  # lead acceleration values
-    y = [0.237, 0.12, 0.027, 0, -0.105, -0.195]  # modification values
-    TR_mod += interp(self.lead_accel_over_time(), x, y)  # todo: test if these modifications are too much
+    x = [-4.4704, -1.77, -0.3145, 0.0, 0.1495, 0.5104, 0.7037, 0.9357]  # lead acceleration values
+    y = [0.237, 0.12, 0.027, 0, -0.006, -0.036, -0.042, -0.045]  # modification values
+    TR_mod += interp(self.lead_accel_over_time(), x, y)
 
     TR += TR_mod
 
     if CS.leftBlinker or CS.rightBlinker:
-      old_TR = float(TR)
       x = [8.9408, 22.352, 31.2928]  # 20, 50, 70 mph
       y = [1.0, .65, .55]  # reduce TR when changing lanes
       TR *= interp(self.car_data['v_ego'], x, y)
-      with open('/data/blinker_debug', 'a') as f:
-        f.write('{}\n'.format([CS.leftBlinker, CS.rightBlinker, self.car_data['v_ego'], old_TR, TR]))
 
     # TR *= self.get_traffic_level()  # modify TR based on last minute of traffic data  # todo: look at getting this to work, a model could be used
 
@@ -181,6 +189,8 @@ def update(self, pm, CS, lead, v_cruise_setpoint):
 
     # Setup current mpc state
     self.cur_state[0].x_ego = 0.0
+    with open("/data/set_speed", "a") as f:
+      f.write("{}".format([v_ego, CS.cruiseState.speed]))
 
     if lead is not None and lead.status:
       x_lead = lead.dRel

selfdrive/controls/lib/longcontrol.py

@@ -2,7 +2,10 @@
 from common.numpy_fast import clip, interp
 from selfdrive.controls.lib.pid import PIController
 from common.travis_checker import travis
-from selfdrive.car.toyota.values import CAR
+from selfdrive.car.toyota.values import CAR as CAR_TOYOTA
+from selfdrive.config import Conversions as CV
+from common.op_params import opParams
+import numpy as np
 
 LongCtrlState = log.ControlsState.LongControlState
 
@@ -67,11 +70,18 @@ def __init__(self, CP, compute_gb, candidate):
                             convert=compute_gb)
     self.v_pid = 0.0
     self.last_output_gb = 0.0
+
+    self.op_params = opParams()
+    self.dynamic_lane_speed_active = self.op_params.get('dynamic_lane_speed', default=True)
+    self.min_dynamic_lane_speed = self.op_params.get('min_dynamic_lane_speed', default=20.) * CV.MPH_TO_MS
+    self.candidate = candidate
+    self.toyota_candidates = [attr for attr in dir(CAR_TOYOTA) if not attr.startswith("__")]
+
+    self.gas_pressed = False
     self.lead_data = {'v_rel': None, 'a_lead': None, 'x_lead': None, 'status': False}
+    self.track_data = []
+    self.mpc_TR = 1.8
     self.v_ego = 0.0
-    self.gas_pressed = False
-    self.candidate = candidate
-    self.toyota_candidates = [attr for attr in dir(CAR) if not attr.startswith("__")]
 
   def reset(self, v_pid):
     """Reset PID controller and change setpoint"""
@@ -80,12 +90,9 @@ def reset(self, v_pid):
 
   def dynamic_gas(self, CP):
     x, y = [], []
-    if CP.enableGasInterceptor:  # if pedal, todo: make different profiles for different vehicles
-      if self.candidate in [CAR.COROLLA]:
-        with open('/data/long_corolla', 'a') as f:
-          f.write('we have liftoff\n')
-        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.35, 0.368, 0.377, 0.389, 0.399, 0.411, 0.45, 0.504, 0.558, 0.617]  # todo: this is the average of the above, only above the 8th index (about .75 reduction)
+    if CP.enableGasInterceptor and (self.candidate in self.toyota_candidates):  # todo: make different profiles for different makes
+      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]  # vels
+      y = [0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229, 0.35, 0.368, 0.377, 0.389, 0.399, 0.411, 0.45, 0.504, 0.558, 0.617]  # max gas
     elif self.candidate in self.toyota_candidates:
       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.35, 0.47, 0.43, 0.35, 0.3, 0.3, 0.3229, 0.34784, 0.36765, 0.38, 0.396, 0.409, 0.425, 0.478, 0.55, 0.621, 0.7]
@@ -95,52 +102,99 @@ def dynamic_gas(self, CP):
 
     gas = interp(self.v_ego, x, y)
 
-    with open('/data/lead_data', 'a') as f:
-      f.write(str(self.lead_data) + '\n')
-
     if self.lead_data['status']:  # if lead
       if self.v_ego <= 8.9408:  # if under 20 mph
-        # TR = 1.8  # desired TR, might need to switch this to hardcoded distance values
-        # current_TR = self.lead_data['x_lead'] / self.v_ego if self.v_ego > 0 else TR
-
         x = [0.0, 0.24588812499999999, 0.432818589, 0.593044697, 0.730381365, 1.050833588, 1.3965, 1.714627481]  # relative velocity mod
         y = [gas * 0.9901, gas * 0.905, gas * 0.8045, gas * 0.625, gas * 0.431, gas * 0.2083, gas * .0667, 0]
         gas_mod = -interp(self.lead_data['v_rel'], x, y)
 
-        # x = [0.0, 0.22, 0.44518483, 0.675, 1.0, 1.76361684]  # lead accel mod  # todo: this
-        # y = [0.0, (gas * 0.08), (gas * 0.20), (gas * 0.4), (gas * 0.52), (gas * 0.6)]
-        # gas_mod += interp(a_lead, x, y)
+        x = [0.44704, 1.78816]  # lead accel mod
+        y = [0.0, gas_mod * .6]
+        gas_mod -= interp(self.lead_data['a_lead'], x, y)  # as lead accelerates, we can reduce the reduction of the above mod (the max this will ouput is the original gas value, it never increases it)
 
         # x = [TR * 0.5, TR, TR * 1.5]  # as lead gets further from car, lessen gas mod  # todo: this
         # y = [gas_mod * 1.5, gas_mod, gas_mod * 0.5]
         # gas_mod += (interp(current_TR, x, y))
-        new_gas = gas + gas_mod  # (interp(current_TR, x, y))
+        new_gas = gas + gas_mod
 
         x = [1.78816, 6.0, 8.9408]  # slowly ramp mods down as we approach 20 mph
         y = [new_gas, (new_gas * 0.8 + gas * 0.2), gas]
         gas = interp(self.v_ego, x, y)
       else:
-        x = [-0.89408, 0, 2.0]  # need to tune this
-        y = [-.17, -.08, .01]
-        gas += interp(self.lead_data['v_rel'], x, y)
+        current_TR = self.lead_data['x_lead'] / self.v_ego
+        x = [-1.78816, -0.89408, 0, 1.78816, 2.68224]  # relative velocity mod
+        y = [-gas * 0.35, -gas * 0.25, -gas * 0.075, gas * 0.175, gas * 0.225]
+        gas_mod = interp(self.lead_data['v_rel'], x, y)
+
+        x = [self.mpc_TR - 0.22, self.mpc_TR, self.mpc_TR + 0.2, self.mpc_TR + 0.4]
+        y = [-gas_mod * 0.36, 0.0, gas_mod * 0.15, gas_mod * 0.45]
+        gas_mod -= interp(current_TR, x, y)
+
+        gas += gas_mod
 
     return clip(gas, 0.0, 1.0)
 
+  def handle_live_tracks(self, live_tracks):
+    if live_tracks['updated']:
+      self.track_data = []
+      for track in live_tracks['tracks']:
+        self.track_data.append({'v_lead': self.v_ego + track.vRel, 'y_rel': track.yRel})
+
   def handle_passable(self, passable):
     self.gas_pressed = passable['gas_pressed']
     self.lead_data['v_rel'] = passable['lead_one'].vRel
     self.lead_data['a_lead'] = passable['lead_one'].aLeadK
     self.lead_data['x_lead'] = passable['lead_one'].dRel
     self.lead_data['status'] = passable['has_lead']  # this fixes radarstate always reporting a lead, thanks to arne
+    self.mpc_TR = passable['mpc_TR']
+    self.handle_live_tracks(passable['live_tracks'])
+
+  def dynamic_lane_speed(self, v_target, v_target_future, v_cruise, a_target):
+    v_cruise *= CV.KPH_TO_MS  # convert to m/s
+    min_tracks = 3
+    vels = [i * CV.MPH_TO_MS for i in [5, 40, 70]]
+    margins = [0.4, 0.55, 0.6]
+    track_speed_margin = interp(self.v_ego, vels, margins)
+    MPC_TIME_STEP = 1 / 20.
+    track_tolerance_v = 0.022352
+    track_tolerance_y = 1.8288
+    if self.dynamic_lane_speed_active and self.v_ego > self.min_dynamic_lane_speed:
+      tracks = []
+      for track in self.track_data:
+        valid = all([True if abs(trk['v_lead'] - track['v_lead']) >= track_tolerance_v else False for trk in tracks])  # radar sometimes reports multiple points for one vehicle, especially semis
+        # valid_y = all([True if abs(trk['y_rel'] - track['y_rel']) >= track_tolerance_y else False for trk in tracks])
+        if valid:  # or valid_y:
+          tracks.append(track)
+      tracks = [trk['v_lead'] for trk in tracks if (self.v_ego * track_speed_margin) <= trk['v_lead'] <= v_cruise]  # .125, 0.025, 0.02500009536743164, 0.02500009536743164
+      if len(tracks) >= min_tracks:
+        average_track_speed = np.mean(tracks)
+        if average_track_speed < v_target and average_track_speed < v_target_future:
+          # so basically, if there's at least 3 tracks, the speeds of the tracks must be within n% of set speed, if our speed is at least set_speed mph,
+          # if the average speeds of tracks is less than v_target and v_target_future, then get a weight for how many tracks exist, with more tracks, the more we
+          # favor the average track speed, then weighted average it with our set_speed, if these conditions aren't met, then we just return original values
+          # this should work...?
+          x = [3, 6, 19]
+          y = [0.275, .375, 0.5]
+          track_speed_weight = interp(len(tracks), x, y)
+          if self.lead_data['status']:  # if lead, give more weight to surrounding tracks (todo: this if check might need to be flipped, so if not lead...)
+            track_speed_weight = clip(1.05 * track_speed_weight, min(y), max(y))
+          v_target_slow = (v_cruise * (1 - track_speed_weight)) + (average_track_speed * track_speed_weight)
+          if v_target_slow < v_target and v_target_slow < v_target_future:  # just a sanity check, don't want to run into any leads if we somehow predict faster velocity
+            a_target_slow = MPC_TIME_STEP * ((v_target_slow - v_target) / 1.0)  # long_mpc runs at 20 hz, so interpolate assuming a_target is 1 second into future? or since long_control is 100hz, should we interpolate using that?
+            a_target = a_target_slow
+            v_target = v_target_slow
+            v_target_future = v_target_slow
+
+    return v_target, v_target_future, a_target
 
   def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP, passable):
     """Update longitudinal control. This updates the state machine and runs a PID loop"""
     self.v_ego = v_ego
-
     # Actuation limits
     if not travis:
       self.handle_passable(passable)
       gas_max = self.dynamic_gas(CP)
+      v_target, v_target_future, a_target = self.dynamic_lane_speed(v_target, v_target_future, v_cruise, a_target)
     else:
       gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV)
     brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV)