Merge pull request #176 from arne182/DP08-clean

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
@@ -278,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]
@@ -325,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')
@@ -341,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

selfdrive/mapd/mapd.py

@@ -22,7 +22,7 @@
 from selfdrive.version import version, dirty
 from common.transformations.coordinates import geodetic2ecef
 from selfdrive.mapd.mapd_helpers import MAPS_LOOKAHEAD_DISTANCE, Way, circle_through_points, rate_curvature_points
-from selfdrive.trafficd.traffic_manager import TrafficdThread
+#from selfdrive.trafficd.traffic_manager import TrafficdThread
 from common.op_params import opParams
 
 traffic_lights = opParams().get('traffic_lights')
@@ -72,8 +72,8 @@ def __init__(self, threadID, name, sharedParams={}): # sharedParams is dict of p
             'Accept-Encoding': 'gzip'
         }
         self.prev_ecef = None
-        self.trafficd_thread = TrafficdThread()
-        self.traffic_light_in_range = False
+        #self.trafficd_thread = TrafficdThread()
+        #self.traffic_light_in_range = False
 
     def is_connected_to_local(self, timeout=3.0):
         try:
@@ -136,11 +136,11 @@ def run(self):
             else:
                 start = time.time()
 
-            if self.trafficd_thread.running and not self.traffic_light_in_range and traffic_lights:
-              self.trafficd_thread.stop()
-            if self.traffic_light_in_range and not self.trafficd_thread.running and traffic_lights:
-              subprocess.call(['pkill','-f','_trafficd'])
-              self.trafficd_thread.start()
+            #if self.trafficd_thread.running and not self.traffic_light_in_range and traffic_lights:
+            #  self.trafficd_thread.stop()
+            #if self.traffic_light_in_range and not self.trafficd_thread.running and traffic_lights:
+            #  subprocess.call(['pkill','-f','_trafficd'])
+            #  self.trafficd_thread.start()
 
             self.logger.debug("Starting after sleeping for 1 second ...")
             last_gps = self.sharedParams.get('last_gps', None)
@@ -233,16 +233,16 @@ def run(self):
                         query_lock.release()
                     else:
                         self.logger.error("There is not query_lock")
-                    self.traffic_light_in_range = False
-                    for n in real_nodes:
-                        if 'highway' in n.tags:
-                            if n.tags['highway'] == 'traffic_signals':
-                                self.traffic_light_in_range = True
-                                break
-                        if 'railway' in n.tags:
-                            if n.tags['railway'] == 'level_crossing':
-                                self.traffic_light_in_range = True
-                                break
+                    #self.traffic_light_in_range = False
+                    #for n in real_nodes:
+                    #    if 'highway' in n.tags:
+                    #        if n.tags['highway'] == 'traffic_signals':
+                    #            self.traffic_light_in_range = True
+                    #            break
+                    #    if 'railway' in n.tags:
+                    #        if n.tags['railway'] == 'level_crossing':
+                    #            self.traffic_light_in_range = True
+                    #            break
 
                 except Exception as e:
                     self.logger.error("ERROR :" + str(e))
@@ -251,13 +251,13 @@ def run(self):
                     query_lock.acquire()
                     self.sharedParams['last_query_result'] = None
                     query_lock.release()
-                    self.traffic_light_in_range = False
+                    #self.traffic_light_in_range = False
             else:
                 query_lock = self.sharedParams.get('query_lock', None)
                 query_lock.acquire()
                 self.sharedParams['last_query_result'] = None
                 query_lock.release()
-                self.traffic_light_in_range = False
+                #self.traffic_light_in_range = False
 
             self.logger.debug("end of one cycle in endless loop ...")
 

selfdrive/ui/paint.cc

@@ -181,8 +181,36 @@ static void update_track_data(UIState *s, const cereal::ModelDataV2::XYZTData::R
 }
 
 static void ui_draw_track(UIState *s, track_vertices_data *pvd) {
-  NVGpaint track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h * .4,
+  if (pvd->cnt == 0) return;
+
+   nvgBeginPath(s->vg);
+   nvgMoveTo(s->vg, pvd->v[0].x, pvd->v[0].y);
+   for (int i=1; i<pvd->cnt; i++) {
+     nvgLineTo(s->vg, pvd->v[i].x, pvd->v[i].y);
+   }
+   nvgClosePath(s->vg);
+
+   NVGpaint track_bg;
+   if (s->scene.controls_state.getEnabled()) {
+     // Draw colored MPC track Kegman's
+     if (s->scene.steerOverride) {
+       track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h*.4,
+         nvgRGBA(0, 191, 255, 255), nvgRGBA(0, 95, 128, 50));
+     } else {
+       int torque_scale = (int)fabs(510*(float)s->scene.output_scale);
+       int red_lvl = fmin(255, torque_scale);
+       int green_lvl = fmin(255, 510-torque_scale);
+       track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h*.4,
+         nvgRGBA(          red_lvl,            green_lvl,  0, 255),
+         nvgRGBA((int)(0.5*red_lvl), (int)(0.5*green_lvl), 0, 50));
+     }
+   } else {
+     // Draw white vision track
+     track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h * .4,
                                         COLOR_WHITE, COLOR_WHITE_ALPHA(0));
+  }
+  nvgFillPaint(s->vg, track_bg);
+  nvgFill(s->vg);
   ui_draw_line(s, &pvd->v[0], pvd->cnt, nullptr, &track_bg);
 }