diff --git a/common/params.cc b/common/params.cc index 38d96a3..d7e3b9b 100644 --- a/common/params.cc +++ b/common/params.cc @@ -207,6 +207,9 @@ std::unordered_map keys = { {"UpdaterTargetBranch", CLEAR_ON_MANAGER_START}, {"UpdaterLastFetchTime", PERSISTENT}, {"Version", PERSISTENT}, + + // FrogPilot parameters + {"ParamConversionVersion", PERSISTENT}, }; } // namespace diff --git a/release/files_common b/release/files_common index 93ab109..d13f3fb 100644 --- a/release/files_common +++ b/release/files_common @@ -565,4 +565,6 @@ tinygrad_repo/tinygrad/runtime/ops_gpu.py tinygrad_repo/tinygrad/shape/* tinygrad_repo/tinygrad/*.py +selfdrive/frogpilot/frogpilot_process.py +selfdrive/frogpilot/controls/frogpilot_planner.py selfdrive/frogpilot/controls/lib/frogpilot_functions.py diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index caa9ef3..c8c9c07 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -840,6 +840,12 @@ class Controls: self.CS_prev = CS + # Update FrogPilot events + self.update_frogpilot_events(CS) + + # Update FrogPilot variables + self.update_frogpilot_variables(CS) + def read_personality_param(self): try: return int(self.params.get('LongitudinalPersonality')) @@ -867,6 +873,15 @@ class Controls: e.set() t.join() + def update_frogpilot_events(self, CS): + + def update_frogpilot_variables(self, CS): + self.driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown) + + fpcc_send = messaging.new_message('frogpilotCarControl') + fpcc_send.valid = CS.canValid + fpcc_send.frogpilotCarControl = self.FPCC + self.pm.send('frogpilotCarControl', fpcc_send) def main(): config_realtime_process(4, Priority.CTRL_HIGH) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 39cfda4..8b11d74 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -58,11 +58,11 @@ STOP_DISTANCE = 6.0 def get_jerk_factor(personality=log.LongitudinalPersonality.standard): if personality==log.LongitudinalPersonality.relaxed: - return 1.0 + return 1.0, 1.0 elif personality==log.LongitudinalPersonality.standard: - return 1.0 + return 1.0, 1.0 elif personality==log.LongitudinalPersonality.aggressive: - return 0.5 + return 0.5, 0.5 else: raise NotImplementedError("Longitudinal personality not supported") @@ -271,11 +271,10 @@ class LongitudinalMpc: for i in range(N): self.solver.cost_set(i, 'Zl', Zl) - def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard): - jerk_factor = get_jerk_factor(personality) + def set_weights(self, acceleration_jerk_factor=1.0, speed_jerk_factor=1.0, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard): if self.mode == 'acc': - a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0 - cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST] + a_change_cost = acceleration_jerk_factor if prev_accel_constraint else 0 + cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, speed_jerk_factor] constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST] elif self.mode == 'blended': a_change_cost = 40.0 if prev_accel_constraint else 0 @@ -330,8 +329,7 @@ class LongitudinalMpc: self.cruise_min_a = min_a self.max_a = max_a - def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard): - t_follow = get_T_FOLLOW(personality) + def update(self, radarstate, v_cruise, x, v, a, j, t_follow, personality=log.LongitudinalPersonality.standard): v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 6cc6e80..6346b2f 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -115,11 +115,12 @@ class LongitudinalPlanner: accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05) accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) - self.mpc.set_weights(prev_accel_constraint, personality=sm['controlsState'].personality) + self.mpc.set_weights(sm['frogpilotPlan'].accelerationJerk, sm['frogpilotPlan'].speedJerk, prev_accel_constraint, personality=sm['controlsState'].personality) self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error) - self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsState'].personality) + self.mpc.update(sm['radarState'], sm['frogpilotPlan'].vCruise, x, v, a, j, sm['frogpilotPlan'].tFollow, + personality=sm['controlsState'].personality) self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.a_solution) diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py new file mode 100644 index 0000000..6d87018 --- /dev/null +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -0,0 +1,82 @@ +import numpy as np + +import cereal.messaging as messaging + +from cereal import car, log + +from openpilot.common.conversions import Conversions as CV +from openpilot.common.numpy_fast import interp +from openpilot.common.params import Params + +from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX +from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_UNSET +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, J_EGO_COST, COMFORT_BRAKE, STOP_DISTANCE, get_jerk_factor, \ + get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW +from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, Lead, get_max_accel + +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CITY_SPEED_LIMIT, CRUISING_SPEED, calculate_lane_width, calculate_road_curvature + +GearShifter = car.CarState.GearShifter + +class FrogPilotPlanner: + def __init__(self): + self.params_memory = Params("/dev/shm/params") + + self.acceleration_jerk = 0 + self.frame = 0 + self.road_curvature = 0 + self.speed_jerk = 0 + + def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, liveLocationKalman, modelData, radarState): + self.lead_one = radarState.leadOne + + v_cruise_kph = min(controlsState.vCruise, V_CRUISE_UNSET) + v_cruise = v_cruise_kph * CV.KPH_TO_MS + + v_ego = max(carState.vEgo, 0) + v_lead = self.lead_one.vLead + + lead_distance = self.lead_one.dRel + + self.base_acceleration_jerk, self.base_speed_jerk = get_jerk_factor(controlsState.personality) + self.t_follow = get_T_FOLLOW(controlsState.personality) + + if self.lead_one.status: + self.update_follow_values(lead_distance, stopping_distance, v_ego, v_lead) + else: + self.acceleration_jerk = self.base_acceleration_jerk + self.speed_jerk = self.base_speed_jerk + + self.road_curvature = calculate_road_curvature(modelData, v_ego) + self.v_cruise = self.update_v_cruise(carState, controlsState, frogpilotCarState, frogpilotNavigation, liveLocationKalman, modelData, v_cruise, v_ego) + + self.frame += 1 + + def update_follow_values(self, lead_distance, stopping_distance, v_ego, v_lead): + + def update_v_cruise(self, carState, controlsState, frogpilotCarState, frogpilotNavigation, liveLocationKalman, modelData, v_cruise, v_ego): + v_cruise_cluster = max(controlsState.vCruiseCluster, controlsState.vCruise) * CV.KPH_TO_MS + v_cruise_diff = v_cruise_cluster - v_cruise + + v_ego_cluster = max(carState.vEgoCluster, v_ego) + v_ego_diff = v_ego_cluster - v_ego + + targets = [] + filtered_targets = [target if target > CRUISING_SPEED else v_cruise for target in targets] + + return min(filtered_targets) + + def publish(self, sm, pm): + frogpilot_plan_send = messaging.new_message('frogpilotPlan') + frogpilot_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) + frogpilotPlan = frogpilot_plan_send.frogpilotPlan + + frogpilotPlan.accelerationJerk = A_CHANGE_COST * float(self.acceleration_jerk) + frogpilotPlan.accelerationJerkStock = A_CHANGE_COST * float(self.base_acceleration_jerk) + frogpilotPlan.speedJerk = J_EGO_COST * float(self.speed_jerk) + frogpilotPlan.speedJerkStock = J_EGO_COST * float(self.base_speed_jerk) + frogpilotPlan.tFollow = float(self.t_follow) + + frogpilotPlan.vCruise = float(self.v_cruise) + + pm.send('frogpilotPlan', frogpilot_plan_send) diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_functions.py b/selfdrive/frogpilot/controls/lib/frogpilot_functions.py index e97b0c9..bdd0ccc 100644 --- a/selfdrive/frogpilot/controls/lib/frogpilot_functions.py +++ b/selfdrive/frogpilot/controls/lib/frogpilot_functions.py @@ -1,10 +1,61 @@ +import datetime import filecmp +import glob +import numpy as np import os import shutil import subprocess from openpilot.common.basedir import BASEDIR +from openpilot.common.numpy_fast import interp +from openpilot.common.params import Params +from openpilot.common.params_pyx import UnknownKeyName from openpilot.system.hardware import HARDWARE +from openpilot.system.version import get_short_branch, get_commit_date + +CITY_SPEED_LIMIT = 25 # 55mph is typically the minimum speed for highways +CRUISING_SPEED = 5 # Roughly the speed cars go when not touching the gas while in drive +PROBABILITY = 0.6 # 60% chance of condition being true +THRESHOLD = 5 # Time threshold (0.25s) + +def calculate_lane_width(lane, current_lane, road_edge): + lane_x, lane_y = np.array(lane.x), np.array(lane.y) + edge_x, edge_y = np.array(road_edge.x), np.array(road_edge.y) + current_x, current_y = np.array(current_lane.x), np.array(current_lane.y) + + lane_y_interp = np.interp(current_x, lane_x[lane_x.argsort()], lane_y[lane_x.argsort()]) + road_edge_y_interp = np.interp(current_x, edge_x[edge_x.argsort()], edge_y[edge_x.argsort()]) + + distance_to_lane = np.mean(np.abs(current_y - lane_y_interp)) + distance_to_road_edge = np.mean(np.abs(current_y - road_edge_y_interp)) + + return min(distance_to_lane, distance_to_road_edge) + +def calculate_road_curvature(modelData, v_ego): + orientation_rate = np.array(np.abs(modelData.orientationRate.z)) + velocity = np.array(modelData.velocity.x) + max_pred_lat_acc = np.amax(orientation_rate * velocity) + return max_pred_lat_acc / (v_ego**2) + +class MovingAverageCalculator: + def __init__(self): + self.data = [] + self.total = 0 + + def add_data(self, value): + if len(self.data) == THRESHOLD: + self.total -= self.data.pop(0) + self.data.append(value) + self.total += value + + def get_moving_average(self): + if len(self.data) == 0: + return None + return self.total / len(self.data) + + def reset_data(self): + self.data = [] + self.total = 0 class FrogPilotFunctions: @classmethod @@ -17,6 +68,61 @@ class FrogPilotFunctions: except Exception as e: print(f"Unexpected error occurred: {e}") + @classmethod + def convert_params(cls, params, params_storage, params_tracking): + if params.get("InstallDate") == "November 21, 2023 - 02:10PM": + params.remove("InstallDate") + + version = 2 + + try: + if params_storage.check_key("ParamConversionVersion"): + current_version = params_storage.get_int("ParamConversionVersion") + if current_version == version: + print("Params already converted, moving on.") + return + print("Converting params...") + except UnknownKeyName: + pass + + def convert_param_mappings(param_mappings, remove_from, min_value=-1): + for key, (getter, setter) in param_mappings.items(): + try: + value = getter(key) + if value > min_value: + setter(key, value) + remove_from.remove(key) + except UnknownKeyName: + pass + + param_mappings = { + "FrogPilotDrives": (params.get_int, params_tracking.put_int), + "FrogPilotKilometers": (params.get_float, params_tracking.put_float), + "FrogPilotMinutes": (params.get_float, params_tracking.put_float) + } + convert_param_mappings(param_mappings, params, 0) + + param_storage_mappings = { + "FrogPilotDrives": (params_storage.get_int, params_tracking.put_int), + "FrogPilotKilometers": (params_storage.get_float, params_tracking.put_float), + "FrogPilotMinutes": (params_storage.get_float, params_tracking.put_float) + } + convert_param_mappings(param_storage_mappings, params_storage, 0) + + def convert_param(key, action_func): + try: + if params_storage.check_key(key): + value = params_storage.get_bool(key) + if value: + action_func() + except UnknownKeyName: + pass + + convert_param("DisableOnroadUploads", lambda: (params.put("DeviceManagement", "True"), params.put("NoUploads", "True"))) + + print("Params successfully converted!") + params_storage.put_int("ParamConversionVersion", version) + @classmethod def setup_frogpilot(cls): frogpilot_boot_logo = f'{BASEDIR}/selfdrive/frogpilot/assets/other_images/frogpilot_boot_logo.png' diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_variables.py b/selfdrive/frogpilot/controls/lib/frogpilot_variables.py new file mode 100644 index 0000000..fd8d6c9 --- /dev/null +++ b/selfdrive/frogpilot/controls/lib/frogpilot_variables.py @@ -0,0 +1,4 @@ +CITY_SPEED_LIMIT = 25 # 55mph is typically the minimum speed for highways +CRUISING_SPEED = 5 # Roughly the speed cars go when not touching the gas while in drive +PROBABILITY = 0.6 # 60% chance of condition being true +THRESHOLD = 5 # Time threshold (0.25s) diff --git a/selfdrive/frogpilot/frogpilot_process.py b/selfdrive/frogpilot/frogpilot_process.py new file mode 100644 index 0000000..171dd01 --- /dev/null +++ b/selfdrive/frogpilot/frogpilot_process.py @@ -0,0 +1,80 @@ +import datetime +import http.client +import os +import socket +import time +import urllib.error +import urllib.request + +import cereal.messaging as messaging + +from cereal import log +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL, Priority, config_realtime_process +from openpilot.common.time import system_time_valid +from openpilot.system.hardware import HARDWARE + +from openpilot.selfdrive.frogpilot.controls.frogpilot_planner import FrogPilotPlanner +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import FrogPilotFunctions + +WIFI = log.DeviceState.NetworkType.wifi + +def github_pinged(url="https://github.com", timeout=5): + try: + urllib.request.urlopen(url, timeout=timeout) + return True + except (urllib.error.URLError, socket.timeout, http.client.RemoteDisconnected): + return False + +def time_checks(deviceState, now, params, params_memory): + screen_off = deviceState.screenBrightnessPercent == 0 + wifi_connection = deviceState.networkType == WIFI + +def frogpilot_thread(): + config_realtime_process(5, Priority.CTRL_LOW) + + params = Params() + params_memory = Params("/dev/shm/params") + + frogpilot_functions = FrogPilotFunctions() + frogpilot_planner = FrogPilotPlanner() + + first_run = True + time_validated = system_time_valid() + + pm = messaging.PubMaster(['frogpilotPlan']) + sm = messaging.SubMaster(['carState', 'controlsState', 'deviceState', 'frogpilotCarControl', 'frogpilotCarState', 'frogpilotNavigation', + 'frogpilotPlan', 'liveLocationKalman', 'longitudinalPlan', 'modelV2', 'radarState'], + poll='modelV2', ignore_avg_freq=['radarState']) + + while True: + sm.update() + + now = datetime.datetime.now() + + deviceState = sm['deviceState'] + started = deviceState.started + + if started and sm.updated['modelV2']: + frogpilot_planner.update(sm['carState'], sm['controlsState'], sm['frogpilotCarControl'], sm['frogpilotCarState'], + sm['frogpilotNavigation'], sm['liveLocationKalman'], sm['modelV2'], sm['radarState']) + frogpilot_planner.publish(sm, pm) + + if not time_validated: + time_validated = system_time_valid() + if not time_validated: + continue + + if now.second == 0 or first_run: + if not started and github_pinged(): + time_checks(deviceState, now, params, params_memory) + + first_run = False + + time.sleep(DT_MDL) + +def main(): + frogpilot_thread() + +if __name__ == "__main__": + main() diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 8ab7c5a..d61388b 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -24,7 +24,19 @@ from openpilot.system.version import is_dirty, get_commit, get_version, get_orig from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import FrogPilotFunctions -def manager_init() -> None: +def frogpilot_boot_functions(frogpilot_functions): + try: + while not system_time_valid(): + print("Waiting for system time to become valid...") + time.sleep(1) + + except Exception as e: + print(f"An unexpected error occurred: {e}") + +def manager_init(frogpilot_functions) -> None: + frogpilot_boot = threading.Thread(target=frogpilot_boot_functions, args=(frogpilot_functions,)) + frogpilot_boot.start() + save_bootlog() params = Params() @@ -34,6 +46,8 @@ def manager_init() -> None: if is_release_branch(): params.clear_all(ParamKeyType.DEVELOPMENT_ONLY) + frogpilot_functions.convert_params(params, params_storage) + default_params: list[tuple[str, str | bytes]] = [ ("CompletedTrainingVersion", "0"), ("DisengageOnAccelerator", "0"), @@ -180,7 +194,7 @@ def manager_thread() -> None: def main(frogpilot_functions) -> None: frogpilot_functions.setup_frogpilot() - manager_init() + manager_init(frogpilot_functions) if os.getenv("PREPAREONLY") is not None: return diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 8b616b7..e9cb143 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -87,6 +87,9 @@ procs = [ NativeProcess("bridge", "cereal/messaging", ["./bridge"], notcar), PythonProcess("webrtcd", "system.webrtc.webrtcd", notcar), PythonProcess("webjoystick", "tools.bodyteleop.web", notcar), + + # FrogPilot processes + PythonProcess("frogpilot_process", "selfdrive.frogpilot.frogpilot_process", always_run), ] managed_processes = {p.name: p for p in procs}