From 3217d4b1642c53aebad37790adac5c94eb5e2087 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Tue, 28 Jan 2025 10:13:33 -0700 Subject: [PATCH] January 28th, 2025 Update --- common/params.cc | 3 + selfdrive/car/hyundai/interface.py | 2 + selfdrive/car/toyota/carcontroller.py | 5 +- selfdrive/classic_modeld/fill_model_msg.py | 4 +- .../controls/lib/frogpilot_vcruise.py | 10 +- .../lib/smart_turn_speed_controller.py | 35 ++++ .../frogpilot/fleetmanager/fleet_manager.py | 85 +++++---- selfdrive/frogpilot/fleetmanager/helpers.py | 32 ++-- .../fleetmanager/templates/tools.html | 60 +++++- selfdrive/frogpilot/frogpilot_process.py | 20 +- selfdrive/frogpilot/frogpilot_utilities.py | 17 +- selfdrive/frogpilot/frogpilot_variables.py | 11 +- .../navigation/ui/primeless_settings.cc | 8 +- .../ui/qt/offroad/device_settings.cc | 23 +-- .../ui/qt/offroad/frogpilot_settings.cc | 2 +- .../ui/qt/offroad/lateral_settings.cc | 4 +- .../ui/qt/offroad/longitudinal_settings.cc | 52 ++---- .../ui/qt/offroad/longitudinal_settings.h | 2 +- .../frogpilot/ui/qt/offroad/model_settings.cc | 4 +- .../frogpilot/ui/qt/offroad/utilities.cc | 28 +++ .../ui/qt/offroad/vehicle_settings.cc | 8 +- .../ui/qt/offroad/visual_settings.cc | 2 +- .../ui/qt/widgets/frogpilot_controls.cc | 12 +- .../ui/qt/widgets/frogpilot_controls.h | 69 +++---- selfdrive/modeld/fill_model_msg.py | 8 +- selfdrive/modeld/parse_model_outputs.py | 1 + selfdrive/ui/ui.cc | 2 +- system/sentry.py | 174 ++++++------------ 28 files changed, 387 insertions(+), 296 deletions(-) diff --git a/common/params.cc b/common/params.cc index 03432ae70b44c0..e1ac9b318e1c81 100644 --- a/common/params.cc +++ b/common/params.cc @@ -285,6 +285,7 @@ std::unordered_map keys = { {"DeviceShutdown", PERSISTENT | FROGPILOT_CONTROLS}, {"DisableOnroadUploads", PERSISTENT | FROGPILOT_CONTROLS}, {"DisableOpenpilotLongitudinal", PERSISTENT | FROGPILOT_VEHICLES}, + {"DiscordUsername", PERSISTENT}, {"DissolvedOxygenDrives", PERSISTENT | FROGPILOT_CONTROLS}, {"DissolvedOxygenScore", PERSISTENT | FROGPILOT_CONTROLS}, {"DistanceIconToDownload", CLEAR_ON_MANAGER_START}, @@ -348,6 +349,7 @@ std::unordered_map keys = { {"IconToDownload", CLEAR_ON_MANAGER_START}, {"IncreasedStoppedDistance", PERSISTENT | FROGPILOT_CONTROLS}, {"IncreaseThermalLimits", PERSISTENT | FROGPILOT_CONTROLS}, + {"IssueReported", CLEAR_ON_MANAGER_START}, {"JerkInfo", PERSISTENT | FROGPILOT_VISUALS}, {"LaneChangeCustomizations", PERSISTENT | FROGPILOT_CONTROLS}, {"LaneChangeTime", PERSISTENT | FROGPILOT_CONTROLS}, @@ -488,6 +490,7 @@ std::unordered_map keys = { {"SLCPriority1", PERSISTENT | FROGPILOT_CONTROLS}, {"SLCPriority2", PERSISTENT | FROGPILOT_CONTROLS}, {"SLCPriority3", PERSISTENT | FROGPILOT_CONTROLS}, + {"SmartTurnControl", PERSISTENT | FROGPILOT_CONTROLS}, {"SNGHack", PERSISTENT | FROGPILOT_VEHICLES}, {"SoundToDownload", CLEAR_ON_MANAGER_START}, {"SpeedLimitAccepted", CLEAR_ON_MANAGER_START}, diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 7ae1679bfd0e34..dc875f5343517b 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -191,6 +191,8 @@ def _update(self, c, frogpilot_toggles): *create_button_events(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT), *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), ] + else: + ret.buttonEvents = create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}) # On some newer model years, the CANCEL button acts as a pause/resume button based on the PCM state # To avoid re-engaging when openpilot cancels, check user engagement intention via buttons diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 96820a4fa00209..bda632f4e98fc9 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -249,8 +249,9 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): pcm_accel_cmd = rate_limit(pcm_accel_cmd, self.prev_accel, ACCEL_WINDDOWN_LIMIT, ACCEL_WINDUP_LIMIT) self.prev_accel = pcm_accel_cmd - # calculate amount of acceleration PCM should apply to reach target, given pitch - accel_due_to_pitch = math.sin(self.pitch.x) * ACCELERATION_DUE_TO_GRAVITY + # calculate amount of acceleration PCM should apply to reach target, given pitch. + # clipped to only include downhill angles, avoids erroneously unsetting PERMIT_BRAKING when stopping on uphills + accel_due_to_pitch = math.sin(min(self.pitch.x, 0.0)) * ACCELERATION_DUE_TO_GRAVITY # TODO: on uphills this sometimes sets PERMIT_BRAKING low not considering the creep force net_acceleration_request = pcm_accel_cmd + accel_due_to_pitch diff --git a/selfdrive/classic_modeld/fill_model_msg.py b/selfdrive/classic_modeld/fill_model_msg.py index c725a0c276ad4a..07f4d723561092 100644 --- a/selfdrive/classic_modeld/fill_model_msg.py +++ b/selfdrive/classic_modeld/fill_model_msg.py @@ -99,14 +99,14 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str, if i < 4: fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['lane_lines'][0,i,:,0], net_output_data['lane_lines'][0,i,:,1]) elif i == 4: - if lane_probs[0] > 0.5: + if lane_probs[0] > 0: leftLane_x = 0.5 * (net_output_data['lane_lines'][0,0,:,0] + net_output_data['lane_lines'][0,1,:,0]) leftLane_y = 0.5 * (net_output_data['lane_lines'][0,0,:,1] + net_output_data['lane_lines'][0,1,:,1]) fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), leftLane_x, leftLane_y) else: fill_xyzt(lane_line, PLAN_T_IDXS, np.empty((0,)), np.empty((0,)), np.empty((0,))) elif i == 5: - if lane_probs[3] > 0.5: + if lane_probs[3] > 0: rightLane_x = 0.5 * (net_output_data['lane_lines'][0,2,:,0] + net_output_data['lane_lines'][0,3,:,0]) rightLane_y = 0.5 * (net_output_data['lane_lines'][0,2,:,1] + net_output_data['lane_lines'][0,3,:,1]) fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), rightLane_x, rightLane_y) diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_vcruise.py b/selfdrive/frogpilot/controls/lib/frogpilot_vcruise.py index efc1ff2fdfc337..02d65cdcf0f88a 100644 --- a/selfdrive/frogpilot/controls/lib/frogpilot_vcruise.py +++ b/selfdrive/frogpilot/controls/lib/frogpilot_vcruise.py @@ -32,6 +32,7 @@ def __init__(self, FrogPilotPlanner): self.slc_offset = 0 self.slc_target = 0 self.speed_limit_timer = 0 + self.stsc_target = 0 self.tracked_model_length = 0 self.vtsc_target = 0 @@ -62,6 +63,10 @@ def update(self, carControl, carState, controlsState, frogpilotCarControl, frogp # FrogsGoMoo's Smart Turn Speed Controller self.stsc.update(carControl, v_cruise, round(v_ego, 2)) + if frogpilot_toggles.smart_turn_speed_controller and v_ego > CRUISING_SPEED and carControl.longActive and self.frogpilot_planner.road_curvature_detected: + self.stsc_target = self.stsc.stsc_target + else: + self.stsc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0 # Pfeiferj's Map Turn Speed Controller if frogpilot_toggles.map_turn_speed_controller and v_ego > CRUISING_SPEED and carControl.longActive: @@ -149,12 +154,13 @@ def update(self, carControl, carState, controlsState, frogpilotCarControl, frogp self.tracked_model_length = self.frogpilot_planner.model_length if frogpilot_toggles.speed_limit_controller: - targets = [self.mtsc_target, max(self.overridden_speed, self.slc_target + self.slc_offset) - v_ego_diff, self.vtsc_target] + targets = [self.mtsc_target, max(self.overridden_speed, self.slc_target + self.slc_offset) - v_ego_diff, self.stsc_target, self.vtsc_target] else: - targets = [self.mtsc_target, self.vtsc_target] + targets = [self.mtsc_target, self.stsc_target, self.vtsc_target] v_cruise = float(min([target if target > CRUISING_SPEED else v_cruise for target in targets])) self.mtsc_target = clip(self.mtsc_target, self.mtsc_target + v_cruise_diff, v_cruise) + self.stsc_target = clip(self.stsc_target, self.mtsc_target + v_cruise_diff, v_cruise) self.vtsc_target = clip(self.vtsc_target, self.mtsc_target + v_cruise_diff, v_cruise) return v_cruise diff --git a/selfdrive/frogpilot/controls/lib/smart_turn_speed_controller.py b/selfdrive/frogpilot/controls/lib/smart_turn_speed_controller.py index faa7657dad0a0a..b0f88acc4e0a11 100644 --- a/selfdrive/frogpilot/controls/lib/smart_turn_speed_controller.py +++ b/selfdrive/frogpilot/controls/lib/smart_turn_speed_controller.py @@ -11,6 +11,7 @@ from openpilot.selfdrive.frogpilot.frogpilot_variables import CRUISING_SPEED, PLANNER_TIME, params +CACHE_WINDOW = 1 CURVATURE_THRESHOLD = 1e-10 ROUNDING_PRECISION = 10 @@ -20,7 +21,40 @@ def __init__(self, FrogPilotVCruise): self.data = SortedDict({entry["speed"]: [np.array([curve["curvature"], curve["lateral_accel"]]) for curve in entry.get("curvatures", [])] for entry in json.loads(params.get("UserCurvature") or "[]")}) + self.last_cached_speed = 0 self.manual_long_timer = 0 + self.stsc_target = 0 + + self.cached_entries = None + self.cached_speeds = None + + def update_cache(self, v_ego): + if abs(self.last_cached_speed - v_ego) <= CACHE_WINDOW / 2: + return + + speeds_in_range = list(self.data.irange(v_ego - CACHE_WINDOW, v_ego + CACHE_WINDOW)) + if speeds_in_range: + self.cached_entries = np.vstack([self.data[speed] for speed in speeds_in_range]) + self.cached_speeds = np.array(speeds_in_range) + else: + self.cached_entries = None + self.cached_speeds = None + + self.last_cached_speed = v_ego + + def set_stsc_target(self, carControl, v_cruise, v_ego): + if not self.data or v_ego < CRUISING_SPEED or not carControl.longActive: + self.stsc_target = v_cruise + return + + self.update_cache(v_ego) + if self.cached_speeds is None: + self.stsc_target = v_cruise + return + + road_curvature = round(self.frogpilot_vcruise.frogpilot_planner.road_curvature, ROUNDING_PRECISION) + closest_entry = min(self.cached_entries, key=lambda x: abs(x[0] - road_curvature)) + self.stsc_target = clip((closest_entry[1] / closest_entry[0])**0.5, CRUISING_SPEED, v_cruise) def update_curvature_data(self, v_ego): road_curvature = round(self.frogpilot_vcruise.frogpilot_planner.road_curvature, ROUNDING_PRECISION) @@ -51,4 +85,5 @@ def update(self, carControl, v_cruise, v_ego): self.manual_long_timer = 0 else: + self.set_stsc_target(carControl, v_cruise, v_ego) self.manual_long_timer = 0 diff --git a/selfdrive/frogpilot/fleetmanager/fleet_manager.py b/selfdrive/frogpilot/fleetmanager/fleet_manager.py index c6d6cbeea942c9..ea83522bd925cd 100644 --- a/selfdrive/frogpilot/fleetmanager/fleet_manager.py +++ b/selfdrive/frogpilot/fleetmanager/fleet_manager.py @@ -171,56 +171,53 @@ def open_error_log(file_name): @app.route("/addr_input", methods=['GET', 'POST']) def addr_input(): preload = fleet.preload_favs() - SearchInput = fleet.get_SearchInput() + search_input = fleet.get_search_input() token = fleet.get_public_token() - s_token = fleet.get_app_token() - gmap_key = fleet.get_gmap_key() - lon = float(0.0) - lat = float(0.0) + + lon = 0.0 + lat = 0.0 + if request.method == 'POST': - valid_addr = False postvars = request.form.to_dict() + valid_addr = False addr, lon, lat, valid_addr, token = fleet.parse_addr(postvars, lon, lat, valid_addr, token) + if not valid_addr: - # If address is not found, try searching - postvars = request.form.to_dict() addr = request.form.get('addr_val') addr, lon, lat, valid_addr, token = fleet.search_addr(postvars, lon, lat, valid_addr, token) + if valid_addr: - # If a valid address is found, redirect to nav_confirmation return redirect(url_for('nav_confirmation', addr=addr, lon=lon, lat=lat)) - else: - return render_template("error.html") - elif has_prime(): + return render_template("error.html") + + if has_prime(): return render_template("prime.html") - # amap stuff - elif SearchInput == 1: - amap_key, amap_key_2 = fleet.get_amap_key() - if amap_key == "" or amap_key is None or amap_key_2 == "" or amap_key_2 is None: - return redirect(url_for('amap_key_input')) - elif token == "" or token is None: + + if search_input == 0: + if fleet.get_public_token() is None: return redirect(url_for('public_token_input')) - elif s_token == "" or s_token is None: + + if fleet.get_secret_token() is None: return redirect(url_for('app_token_input')) - else: - return redirect(url_for('amap_addr_input')) - elif fleet.get_nav_active(): - if SearchInput == 2: - return render_template("nonprime.html", gmap_key=gmap_key, lon=lon, lat=lat, home=preload[0], work=preload[1], fav1=preload[2], fav2=preload[3], fav3=preload[4]) - else: - return render_template("nonprime.html", gmap_key=None, lon=None, lat=None, home=preload[0], work=preload[1], fav1=preload[2], fav2=preload[3], fav3=preload[4]) - elif token == "" or token is None: - return redirect(url_for('public_token_input')) - elif s_token == "" or s_token is None: - return redirect(url_for('app_token_input')) - elif SearchInput == 2: + + if search_input == 1: + amap_key, amap_key_2 = fleet.get_amap_key() + if not amap_key or not amap_key_2: + return redirect(url_for('amap_key_input')) + return redirect(url_for('amap_addr_input')) + + if search_input == 2: + gmap_key = fleet.get_gmap_key() lon, lat = fleet.get_last_lon_lat() - if gmap_key == "" or gmap_key is None: + + if not gmap_key: return redirect(url_for('gmap_key_input')) - else: - return render_template("addr.html", gmap_key=gmap_key, lon=lon, lat=lat, home=preload[0], work=preload[1], fav1=preload[2], fav2=preload[3], fav3=preload[4]) - else: - return render_template("addr.html", gmap_key=None, lon=None, lat=None, home=preload[0], work=preload[1], fav1=preload[2], fav2=preload[3], fav3=preload[4]) + return render_template("addr.html", gmap_key=gmap_key, lon=lon, lat=lat, home=preload[0], work=preload[1], fav1=preload[2], fav2=preload[3], fav3=preload[4]) + + if fleet.get_nav_active(): + return render_template("nonprime.html", gmap_key=None, lon=None, lat=None, home=preload[0], work=preload[1], fav1=preload[2], fav2=preload[3], fav3=preload[4]) + + return render_template("addr.html", gmap_key=None, lon=None, lat=None, home=preload[0], work=preload[1], fav1=preload[2], fav2=preload[3], fav3=preload[4]) @app.route("/nav_confirmation", methods=['GET', 'POST']) def nav_confirmation(): @@ -328,6 +325,14 @@ def get_toggle_values_route(): toggle_values = fleet.get_all_toggle_values() return jsonify(toggle_values) +@app.route("/reset_toggle_values", methods=['POST']) +def reset_toggle_values_route(): + try: + fleet.reset_toggle_values() + return jsonify({"message": "Toggles reset successfully! Rebooting..."}), 200 + except Exception as error: + return jsonify({"error": "Failed to reset toggles...", "details": str(error)}), 400 + @app.route("/store_toggle_values", methods=['POST']) def store_toggle_values_route(): try: @@ -353,6 +358,14 @@ def unlock_doors_route(): except Exception as error: return jsonify({"error": "Failed to unlock doors...", "details": str(error)}), 400 +@app.route("/reboot_device", methods=['POST']) +def reboot_device_route(): + try: + fleet.reboot_device() + return jsonify({"message": "Successfully rebooted!"}), 200 + except Exception as error: + return jsonify({"error": "Failed to reboot...", "details": str(error)}), 400 + def main(): try: set_core_affinity([0, 1, 2, 3]) diff --git a/selfdrive/frogpilot/fleetmanager/helpers.py b/selfdrive/frogpilot/fleetmanager/helpers.py index 1abcab84723b42..71114a94494b7e 100644 --- a/selfdrive/frogpilot/fleetmanager/helpers.py +++ b/selfdrive/frogpilot/fleetmanager/helpers.py @@ -35,7 +35,7 @@ from openpilot.common.params import ParamKeyType from openpilot.selfdrive.car.toyota.carcontroller import LOCK_CMD, UNLOCK_CMD -from openpilot.system.hardware import PC +from openpilot.system.hardware import HARDWARE, PC from openpilot.system.hardware.hw import Paths from openpilot.system.loggerd.uploader import listdir_by_creation from openpilot.system.loggerd.xattr_cache import getxattr @@ -227,23 +227,24 @@ def get_nav_active(): return False def get_amap_key(): - return ( - token.strip() if (token := params.get("AMapKey1", encoding='utf8')) != "0" else None, - token2.strip() if (token2 := params.get("AMapKey2", encoding='utf8')) != "0" else None - ) + token = params.get("AMapKey1", encoding='utf8') + token2 = params.get("AMapKey2", encoding='utf8') + return (token.strip() if token else None, token2.strip() if token2 else None) def get_gmap_key(): - return token.strip() if (token := params.get("GMapKey", encoding='utf8')) != "0" else None + token = params.get("GMapKey", encoding='utf8') + return token.strip() if token else None def get_public_token(): - return token.strip() if (token := params.get("MapboxPublicKey", encoding='utf8')).startswith("pk") else None + token = params.get("MapboxPublicKey", encoding='utf8') + return token.strip() if token and token.startswith("pk") else None -def get_app_token(): - return token.strip() if (token := params.get("MapboxSecretKey", encoding='utf8')).startswith("sk") else None +def get_secret_token(): + token = params.get("MapboxSecretKey", encoding='utf8') + return token.strip() if token and token.startswith("sk") else None -def get_SearchInput(): - SearchInput = params.get_int("SearchInput") - return SearchInput +def get_search_input(): + return params.get_int("SearchInput") def get_last_lon_lat(): last_pos = params.get("LastGPSPosition") @@ -467,6 +468,10 @@ def get_all_toggle_values(): return encode_parameters(toggle_values) +def reset_toggle_values(): + params.put_bool("DoToggleReset", True) + HARDWARE.reboot() + def store_toggle_values(request_data): excluded_keys = [ "ApiCache_NavDestinations", "CalibrationParams", "CarParamsPersistent", @@ -495,3 +500,6 @@ def unlock_doors(): panda.can_send(0x750, UNLOCK_CMD, 0) panda.set_safety_mode(panda.SAFETY_TOYOTA) panda.send_heartbeat() + +def reboot_device(): + HARDWARE.reboot() diff --git a/selfdrive/frogpilot/fleetmanager/templates/tools.html b/selfdrive/frogpilot/fleetmanager/templates/tools.html index a26c0b8399c827..4f25863e8426fc 100644 --- a/selfdrive/frogpilot/fleetmanager/templates/tools.html +++ b/selfdrive/frogpilot/fleetmanager/templates/tools.html @@ -75,7 +75,7 @@
-

Toggle Values

+

Backup/Restore Toggle Values

+ +
+ +

Reset Toggle Values

+
+ +
+ +
+ +

Reboot Device

+
+ +