diff --git a/Dockerfile.openpilot_base b/Dockerfile.openpilot_base index c3c8f8cd15d98f..367d2add73b3f7 100644 --- a/Dockerfile.openpilot_base +++ b/Dockerfile.openpilot_base @@ -14,6 +14,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ cppcheck \ curl \ ffmpeg \ + gcc-arm-none-eabi \ git \ iputils-ping \ libarchive-dev \ 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/scripts/update_now.py b/scripts/update_now.py new file mode 100644 index 00000000000000..91b539252f487d --- /dev/null +++ b/scripts/update_now.py @@ -0,0 +1,15 @@ +#!/usr/bin/env python3 +import datetime +from common.params import Params +from selfdrive.data_collection import gps_uploader + +print("Don't forget to pray!") +params = Params() +t = datetime.datetime.utcnow().isoformat() +params.put("LastUpdateTime", t.encode('utf8')) +if params.get("IsOffroad") == b"1": + print("Please wait for gps to upload to aviod this in future!") + gps_uploader.upload_data() +else: + print("Please switch off car and try again!") + 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..078faa0715506a 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.]] @@ -81,13 +81,12 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.steerRatio = 13.4 # True steerRation from older prius 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 + ret.steerActuatorDelay = 0.6 if prius_pid: ret.lateralTuning.init('pid') ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.35], [0.1]] - ret.lateralTuning.pid.kdV = [2.0] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.35, 0.18], [0.03, 0.1]] + ret.lateralTuning.pid.kdV = [2.2] ret.lateralTuning.pid.kfV = [0.00007818594] ret.lateralTuning.pid.newKfTuned = False else: @@ -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 @@ -269,7 +270,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.safetyParam = 56 ret.wheelbase = 2.68986 ret.steerRatio = 14.3 - ret.steerRateCost = 0.1 + ret.steerRateCost = 0.4 tire_stiffness_factor = 0.7933 ret.mass = 3370. * CV.LB_TO_KG + STD_CARGO_KG ret.longitudinalTuning.deadzoneBP = [0., 8.05] @@ -278,20 +279,20 @@ 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.startingBrakeRate = 0.9 # release brakes fast + ret.stoppingBrakeRate = 0.15 # reach stopping target smoothly + ret.startingBrakeRate = 1.21 # release brakes fast ret.startAccel = 1.50 # Accelerate from 0 faster ret.steerActuatorDelay = 0 - ret.steerLimitTimer = 0.4 + ret.steerLimitTimer = 5 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.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] - ret.lateralTuning.indi.actuatorEffectivenessV = [15, 15] + ret.lateralTuning.indi.innerLoopGainBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 26] + ret.lateralTuning.indi.innerLoopGainV = [4.15, 5.55, 7, 8.9, 10.0, 12.15, 14.25, 15] + ret.lateralTuning.indi.outerLoopGainBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 26, 30.3, 33.6] + ret.lateralTuning.indi.outerLoopGainV = [3.6, 4.9, 6.15, 7.5, 10, 11.2, 15, 16, 17, 17] + ret.lateralTuning.indi.timeConstantBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 26, 30.3, 33.6] + ret.lateralTuning.indi.timeConstantV = [0.46, 0.62, 0.77, 0.93, 1.1, 3.0, 4.0, 5.5, 7.0, 8.0] + ret.lateralTuning.indi.actuatorEffectivenessBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 26] + ret.lateralTuning.indi.actuatorEffectivenessV = [4.15, 5.55, 7, 8.9, 10.0, 12.15, 14.25, 15] elif candidate == CAR.RAV4H_TSS2: stop_and_go = True @@ -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..b49f2c78c7ad44 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,18 @@ 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 + 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 +262,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 +355,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 and distance_traveled: + 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 +383,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 +496,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/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py index 1bd2c5c403983c..58e1eaa05b1415 100644 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ b/selfdrive/controls/lib/latcontrol_lqr.py @@ -48,9 +48,9 @@ def update(self, active, CS, CP, path_plan): steers_max = get_steer_max(CP, CS.vEgo) if (self.output_steer > 0.0) and (CS.steeringAngle > 0.0): - factor = -0.4 + factor = -0.2 elif (self.output_steer < 0.0) and (CS.steeringAngle < 0.0): - factor = -0.4 + factor = -0.2 else: factor = 0.0 torque_scale = (1-factor*max(abs(CS.steeringAngle)/100,1.0))*(0.45 + CS.vEgo / 60.0)**2 # Scale actuator model with speed diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 9b942df68e6f19..90595a7020414c 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): # 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 f2beab6f90438d..c21cb68877d375 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/data_collection/gps_uploader.py b/selfdrive/data_collection/gps_uploader.py index 665ab557b6b53f..befcd3d342010a 100644 --- a/selfdrive/data_collection/gps_uploader.py +++ b/selfdrive/data_collection/gps_uploader.py @@ -48,7 +48,7 @@ def upload_data(): ftp.storbinary("STOR /{}/{}".format(username, filename + ".gz"), f) ftp.quit() os.remove(filepath) - os.remove("/data/" + filename + ".gz") + os.remove("/data/*.gz") t = datetime.datetime.utcnow().isoformat() params.put("LastUpdateTime", t.encode('utf8')) return True 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/thermald/thermald.py b/selfdrive/thermald/thermald.py index d9fdeead57425e..83d5211617164e 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -#import datetime +import datetime import os import time from collections import namedtuple @@ -345,45 +345,45 @@ def thermald_thread(): # **** starting logic **** # Check for last update time and display alerts if needed - # now = datetime.datetime.utcnow() + now = datetime.datetime.utcnow() # # # show invalid date/time alert - # startup_conditions["time_valid"] = (now.year > 2020) or (now.year == 2020 and now.month >= 10) - # set_offroad_alert_if_changed("Offroad_InvalidTime", (not startup_conditions["time_valid"])) + startup_conditions["time_valid"] = (now.year > 2020) or (now.year == 2020 and now.month >= 10) + set_offroad_alert_if_changed("Offroad_InvalidTime", (not startup_conditions["time_valid"])) # # # Show update prompt - # try: - # last_update = datetime.datetime.fromisoformat(params.get("LastUpdateTime", encoding='utf8')) - # except (TypeError, ValueError): - # last_update = now - # dt = now - last_update - # - # update_failed_count = params.get("UpdateFailedCount") - # update_failed_count = 0 if update_failed_count is None else int(update_failed_count) - # last_update_exception = params.get("LastUpdateException", encoding='utf8') + try: + last_update = datetime.datetime.fromisoformat(params.get("LastUpdateTime", encoding='utf8')) + except (TypeError, ValueError): + last_update = now + dt = now - last_update # - # if update_failed_count > 15 and last_update_exception is not None: - # if current_branch in ["release2", "dashcam"]: - # extra_text = "Ensure the software is correctly installed" - # else: - # extra_text = last_update_exception + update_failed_count = params.get("UpdateFailedCount") + update_failed_count = 0 if update_failed_count is None else int(update_failed_count) + last_update_exception = params.get("LastUpdateException", encoding='utf8') # - # set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) - # set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) - # set_offroad_alert_if_changed("Offroad_UpdateFailed", True, extra_text=extra_text) - # elif dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1: - # set_offroad_alert_if_changed("Offroad_UpdateFailed", False) - # set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) - # set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", True) - # elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT: - # remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0)) - # set_offroad_alert_if_changed("Offroad_UpdateFailed", False) - # set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) - # set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining_time} days.") - # else: - # set_offroad_alert_if_changed("Offroad_UpdateFailed", False) - # set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) - # set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) + if update_failed_count > 15 and last_update_exception is not None: + if current_branch in ["release5", "dashcam"]: + extra_text = "Ensure the software is correctly installed" + else: + extra_text = last_update_exception + + set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) + set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) + set_offroad_alert_if_changed("Offroad_UpdateFailed", True, extra_text=extra_text) + elif dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1: + set_offroad_alert_if_changed("Offroad_UpdateFailed", False) + set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) + set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", True) + elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT: + remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0)) + set_offroad_alert_if_changed("Offroad_UpdateFailed", False) + set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) + set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining_time} days.") + else: + set_offroad_alert_if_changed("Offroad_UpdateFailed", False) + set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) + set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) startup_conditions["not_uninstalling"] = not params.get("DoUninstall") == b"1" startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version 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..3cd1541085b73f 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() + + scene.controls_state.getLateralControlState().getLqrState().getOutput() + + 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..17048cdda7ed6d 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -107,9 +107,9 @@ def set_params(new_version: bool, failed_count: int, exception: Optional[str]) - params = Params() params.put("UpdateFailedCount", str(failed_count)) - if failed_count == 0: - t = datetime.datetime.utcnow().isoformat() - params.put("LastUpdateTime", t.encode('utf8')) + #if failed_count == 0: + #t = datetime.datetime.utcnow().isoformat() + #params.put("LastUpdateTime", t.encode('utf8')) if exception is None: params.delete("LastUpdateException") @@ -120,7 +120,7 @@ def set_params(new_version: bool, failed_count: int, exception: Optional[str]) - branch_name = run(["git", "rev-parse", "--abbrev-ref", "HEAD"], FINALIZED).rstrip() if branch_name == "testing": postfix = '' - elif branch_name == "devel-i18n": + elif branch_name == "DP08-clean": postfix = '-DEV' else: postfix = '-REL' @@ -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