diff --git a/common/op_params.py b/common/op_params.py index 2168e1c6854de9..b3ddddffec2d84 100644 --- a/common/op_params.py +++ b/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" diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index 5c1c97184b852f..7193e3a09eb10b 100755 --- a/common/params_pyx.pyx +++ b/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], diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 8193c6076c96d6..88ddb8aa90f163 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/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) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 2c6d4420cccd00..5e812b8910826e 100755 --- a/selfdrive/car/toyota/interface.py +++ b/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]: diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index eac7b10008846d..5156cfbce7191a 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/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, } diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 9754f5f898c2e3..5e9587ea543f28 100755 --- a/selfdrive/controls/controlsd.py +++ b/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) diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 9b942df68e6f19..c35729ee23fbca 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/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 diff --git a/selfdrive/crash.py b/selfdrive/crash.py index 27f6dab18b0994..137b0365d89081 100644 --- a/selfdrive/crash.py +++ b/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): diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 5e5cb1aa1fb0f6..325c4d76d694c2 100755 --- a/selfdrive/manager.py +++ b/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 diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py index 9956a56f085404..e150be77fc22dc 100755 --- a/selfdrive/mapd/mapd.py +++ b/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 ...") diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 5a45f20521de33..0679aecf4033f6 100644 --- a/selfdrive/ui/paint.cc +++ b/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; icnt; 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); } diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index e6ce6b1683ba56..57fdd2b2a5396a 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -123,6 +123,11 @@ void update_sockets(UIState *s) { scene.controls_state = event.getControlsState(); // TODO: the alert stuff shouldn't be handled here + s->scene.steerOverride= scene.controls_state.getSteerOverride(); + s->scene.output_scale = scene.controls_state.getLateralControlState().getPidState().getOutput(); + s->scene.output_scale = scene.controls_state.getLateralControlState().getLqrState().getOutput(); + s->scene.output_scale = scene.controls_state.getLateralControlState().getIndiState().getOutput(); + auto alert_sound = scene.controls_state.getAlertSound(); if (scene.alert_type.compare(scene.controls_state.getAlertType()) != 0) { if (alert_sound == AudibleAlert::NONE) { diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index 6fc5d0499b2d83..86ff8f07ec24f6 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -178,6 +178,8 @@ typedef struct UIScene { bool rightBlinker; bool brakeLights; int blinker_blinkingrate; + bool steerOverride; + float output_scale; // for blind spot bool leftBlindspot; bool rightBlindspot; diff --git a/selfdrive/updated.py b/selfdrive/updated.py index dbb1791906d659..8f812db6bf778c 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -323,7 +323,7 @@ def fetch_update(wait_helper: WaitTimeHelper) -> bool: class AutoReboot: def __init__(self): - self.min_reboot_time = 5. * 60 + self.min_reboot_time = 5. * 30 self.need_reboot = False self.time_offroad = 0.0