FrogPilot setup - FrogPilot functions

common/params.cc

@@ -207,6 +207,9 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"UpdaterTargetBranch", CLEAR_ON_MANAGER_START},
     {"UpdaterLastFetchTime", PERSISTENT},
     {"Version", PERSISTENT},
+
+    // FrogPilot parameters
+    {"ParamConversionVersion", PERSISTENT},
 };
 
 } // namespace

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

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)

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
 

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)

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)

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'

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)

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()

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
 

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}