FrogPilot 0.9.7

selfdrive/frogpilot/controls/frogpilot_planner.py

@@ -0,0 +1,149 @@
+#!/usr/bin/env python3
+import cereal.messaging as messaging
+
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.realtime import DT_MDL
+
+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, DANGER_ZONE_COST, J_EGO_COST, STOP_DISTANCE
+from openpilot.selfdrive.controls.lib.longitudinal_planner import Lead
+
+from openpilot.selfdrive.frogpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode
+from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_acceleration import FrogPilotAcceleration
+from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_events import FrogPilotEvents
+from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_following import FrogPilotFollowing
+from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_vcruise import FrogPilotVCruise
+from openpilot.selfdrive.frogpilot.frogpilot_utilities import calculate_lane_width, calculate_road_curvature
+from openpilot.selfdrive.frogpilot.frogpilot_variables import CRUISING_SPEED, MODEL_LENGTH, NON_DRIVING_GEARS, PLANNER_TIME, THRESHOLD
+
+class FrogPilotPlanner:
+  def __init__(self, error_log):
+    self.error_log = error_log
+
+    self.cem = ConditionalExperimentalMode(self)
+    self.frogpilot_acceleration = FrogPilotAcceleration(self)
+    self.frogpilot_events = FrogPilotEvents(self)
+    self.frogpilot_following = FrogPilotFollowing(self)
+    self.frogpilot_vcruise = FrogPilotVCruise(self)
+    self.lead_one = Lead()
+
+    self.tracking_lead_filter = FirstOrderFilter(0, 1, DT_MDL)
+
+    self.lateral_check = False
+    self.model_stopped = False
+    self.road_curvature_detected = False
+    self.slower_lead = False
+    self.tracking_lead = False
+
+    self.model_length = 0
+    self.road_curvature = 1
+    self.v_cruise = 0
+
+  def update(self, carControl, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, modelData, radarless_model, radarState, frogpilot_toggles):
+    if radarless_model:
+      model_leads = list(modelData.leadsV3)
+      if len(model_leads) > 0:
+        distance_offset = frogpilot_toggles.increased_stopped_distance if not frogpilotCarState.trafficModeActive else 0
+        model_lead = model_leads[0]
+        self.lead_one.update(model_lead.x[0] - distance_offset, model_lead.y[0], model_lead.v[0], model_lead.a[0], model_lead.prob)
+      else:
+        self.lead_one.reset()
+    else:
+      self.lead_one = radarState.leadOne
+
+    v_cruise = min(controlsState.vCruise, V_CRUISE_UNSET) * CV.KPH_TO_MS
+    v_ego = max(carState.vEgo, 0)
+    v_lead = self.lead_one.vLead
+
+    self.frogpilot_acceleration.update(frogpilotCarState, v_ego, frogpilot_toggles)
+
+    run_cem = frogpilot_toggles.conditional_experimental_mode or frogpilot_toggles.force_stops or frogpilot_toggles.green_light_alert or frogpilot_toggles.show_stopping_point
+    if run_cem and (controlsState.enabled or frogpilotCarControl.alwaysOnLateralActive) and carState.gearShifter not in NON_DRIVING_GEARS:
+      self.cem.update(carState, frogpilotCarState, frogpilotNavigation, modelData, v_ego, v_lead, frogpilot_toggles)
+    else:
+      self.cem.stop_light_detected = False
+
+    self.frogpilot_events.update(carState, controlsState, frogpilotCarControl, frogpilotCarState, self.lead_one.dRel, modelData, v_lead, frogpilot_toggles)
+    self.frogpilot_following.update(carState.aEgo, controlsState, frogpilotCarState, self.lead_one.dRel, v_ego, v_lead, frogpilot_toggles)
+
+    check_lane_width = frogpilot_toggles.adjacent_paths or frogpilot_toggles.adjacent_path_metrics or frogpilot_toggles.blind_spot_path or frogpilot_toggles.lane_detection
+    if check_lane_width and v_ego >= frogpilot_toggles.minimum_lane_change_speed or frogpilot_toggles.adjacent_lead_tracking:
+      self.lane_width_left = calculate_lane_width(modelData.laneLines[0], modelData.laneLines[1], modelData.roadEdges[0])
+      self.lane_width_right = calculate_lane_width(modelData.laneLines[3], modelData.laneLines[2], modelData.roadEdges[1])
+    else:
+      self.lane_width_left = 0
+      self.lane_width_right = 0
+
+    self.lateral_check = v_ego >= frogpilot_toggles.pause_lateral_below_speed
+    self.lateral_check |= frogpilot_toggles.pause_lateral_below_signal and not (carState.leftBlinker or carState.rightBlinker)
+    self.lateral_check |= carState.standstill
+
+    self.model_length = modelData.position.x[MODEL_LENGTH - 1]
+    self.model_stopped = self.model_length < CRUISING_SPEED * PLANNER_TIME
+    self.model_stopped |= self.frogpilot_vcruise.forcing_stop
+
+    self.road_curvature = calculate_road_curvature(modelData, v_ego) if not carState.standstill else 1
+    self.road_curvature_detected = (1 / self.road_curvature)**0.5 < v_ego
+
+    self.tracking_lead = self.set_lead_status(carState, v_lead)
+    self.v_cruise = self.frogpilot_vcruise.update(carControl, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, v_cruise, v_ego, frogpilot_toggles)
+
+  def set_lead_status(self, carState, v_lead):
+    following_lead = self.lead_one.status
+    following_lead &= self.lead_one.dRel < self.model_length + STOP_DISTANCE
+    following_lead &= not carState.standstill or self.tracking_lead
+
+    self.tracking_lead_filter.update(following_lead)
+    return self.tracking_lead_filter.x >= THRESHOLD**2
+
+  def publish(self, sm, pm, toggles_updated):
+    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 = float(A_CHANGE_COST * self.frogpilot_following.acceleration_jerk)
+    frogpilotPlan.accelerationJerkStock = float(A_CHANGE_COST * self.frogpilot_following.base_acceleration_jerk)
+    frogpilotPlan.dangerJerk = float(DANGER_ZONE_COST * self.frogpilot_following.danger_jerk)
+    frogpilotPlan.speedJerk = float(J_EGO_COST * self.frogpilot_following.speed_jerk)
+    frogpilotPlan.speedJerkStock = float(J_EGO_COST * self.frogpilot_following.base_speed_jerk)
+    frogpilotPlan.tFollow = float(self.frogpilot_following.t_follow)
+
+    frogpilotPlan.mtscSpeed = float(self.frogpilot_vcruise.mtsc_target)
+    frogpilotPlan.vtscControllingCurve = bool(self.frogpilot_vcruise.mtsc_target > self.frogpilot_vcruise.vtsc_target)
+    frogpilotPlan.vtscSpeed = float(self.frogpilot_vcruise.vtsc_target)
+
+    frogpilotPlan.desiredFollowDistance = self.frogpilot_following.desired_follow_distance
+
+    frogpilotPlan.experimentalMode = self.cem.experimental_mode or self.frogpilot_vcruise.slc.experimental_mode
+
+    frogpilotPlan.forcingStop = self.frogpilot_vcruise.forcing_stop
+    frogpilotPlan.forcingStopLength = self.frogpilot_vcruise.tracked_model_length
+
+    frogpilotPlan.frogpilotEvents = self.frogpilot_events.events.to_msg()
+
+    frogpilotPlan.laneWidthLeft = self.lane_width_left
+    frogpilotPlan.laneWidthRight = self.lane_width_right
+
+    frogpilotPlan.lateralCheck = self.lateral_check
+
+    frogpilotPlan.maxAcceleration = float(self.frogpilot_acceleration.max_accel)
+    frogpilotPlan.minAcceleration = float(self.frogpilot_acceleration.min_accel)
+
+    frogpilotPlan.redLight = bool(self.cem.stop_light_detected)
+
+    frogpilotPlan.slcMapSpeedLimit = self.frogpilot_vcruise.slc.map_speed_limit
+    frogpilotPlan.slcOverridden = bool(self.frogpilot_vcruise.override_slc)
+    frogpilotPlan.slcOverriddenSpeed = float(self.frogpilot_vcruise.overridden_speed)
+    frogpilotPlan.slcSpeedLimit = self.frogpilot_vcruise.slc_target
+    frogpilotPlan.slcSpeedLimitOffset = self.frogpilot_vcruise.slc_offset
+    frogpilotPlan.slcSpeedLimitSource = self.frogpilot_vcruise.slc.source
+    frogpilotPlan.speedLimitChanged = self.frogpilot_vcruise.slc.speed_limit_changed
+    frogpilotPlan.unconfirmedSlcSpeedLimit = self.frogpilot_vcruise.slc.desired_speed_limit
+    frogpilotPlan.upcomingSLCSpeedLimit = self.frogpilot_vcruise.slc.upcoming_speed_limit
+
+    frogpilotPlan.togglesUpdated = toggles_updated
+
+    frogpilotPlan.vCruise = self.v_cruise
+
+    pm.send('frogpilotPlan', frogpilot_plan_send)

selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py

@@ -0,0 +1,100 @@
+#!/usr/bin/env python3
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.realtime import DT_MDL
+
+from openpilot.selfdrive.frogpilot.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, THRESHOLD, params_memory
+
+class ConditionalExperimentalMode:
+  def __init__(self, FrogPilotPlanner):
+    self.frogpilot_planner = FrogPilotPlanner
+
+    self.curvature_filter = FirstOrderFilter(0, 1, DT_MDL)
+    self.slow_lead_filter = FirstOrderFilter(0, 1, DT_MDL)
+    self.stop_light_filter = FirstOrderFilter(0, 1, DT_MDL)
+
+    self.curve_detected = False
+    self.experimental_mode = False
+    self.stop_light_detected = False
+
+  def update(self, carState, frogpilotCarState, frogpilotNavigation, modelData, v_ego, v_lead, frogpilot_toggles):
+    self.status_value = params_memory.get_int("CEStatus")
+
+    if self.status_value not in {1, 2, 3, 4, 5, 6} and not carState.standstill:
+      self.update_conditions(frogpilotCarState, self.frogpilot_planner.tracking_lead, v_ego, v_lead, frogpilot_toggles)
+      self.experimental_mode = self.check_conditions(carState, frogpilotNavigation, modelData, self.frogpilot_planner.frogpilot_following.following_lead, v_ego, v_lead, frogpilot_toggles)
+      params_memory.put_int("CEStatus", self.status_value if self.experimental_mode else 0)
+    else:
+      self.experimental_mode = self.status_value in {2, 4, 6} or carState.standstill and self.experimental_mode and self.frogpilot_planner.model_stopped
+      self.stop_light_detected &= self.status_value not in {1, 2, 3, 4, 5, 6}
+      self.stop_light_filter.x = 0
+
+  def check_conditions(self, carState, frogpilotNavigation, modelData, following_lead, v_ego, v_lead, frogpilot_toggles):
+    below_speed = frogpilot_toggles.conditional_limit > v_ego >= 1 and not following_lead
+    below_speed_with_lead = frogpilot_toggles.conditional_limit_lead > v_ego >= 1 and following_lead
+    if below_speed or below_speed_with_lead:
+      self.status_value = 7 if following_lead else 8
+      return True
+
+    desired_lane = self.frogpilot_planner.lane_width_left if carState.leftBlinker else self.frogpilot_planner.lane_width_right
+    lane_available = desired_lane >= frogpilot_toggles.lane_detection_width or not frogpilot_toggles.conditional_signal_lane_detection
+    if v_ego < frogpilot_toggles.conditional_signal and (carState.leftBlinker or carState.rightBlinker) and not lane_available:
+      self.status_value = 9
+      return True
+
+    approaching_maneuver = modelData.navEnabled and (frogpilotNavigation.approachingIntersection or frogpilotNavigation.approachingTurn)
+    if frogpilot_toggles.conditional_navigation and approaching_maneuver and (frogpilot_toggles.conditional_navigation_lead or not following_lead):
+      self.status_value = 10 if frogpilotNavigation.approachingIntersection else 11
+      return True
+
+    if frogpilot_toggles.conditional_curves and self.curve_detected and (frogpilot_toggles.conditional_curves_lead or not following_lead):
+      self.status_value = 12
+      return True
+
+    if frogpilot_toggles.conditional_lead and self.slow_lead_detected:
+      self.status_value = 13 if v_lead < 1 else 14
+      return True
+
+    if frogpilot_toggles.conditional_model_stop_time != 0 and self.stop_light_detected:
+      self.status_value = 15 if not self.frogpilot_planner.frogpilot_vcruise.forcing_stop else 16
+      return True
+
+    if self.frogpilot_planner.frogpilot_vcruise.slc.experimental_mode:
+      self.status_value = 17
+      return True
+
+    return False
+
+  def update_conditions(self, frogpilotCarState, tracking_lead, v_ego, v_lead, frogpilot_toggles):
+    self.curve_detection(tracking_lead, v_ego, frogpilot_toggles)
+    self.slow_lead(tracking_lead, v_lead, frogpilot_toggles)
+    self.stop_sign_and_light(frogpilotCarState, tracking_lead, v_ego, frogpilot_toggles)
+
+  def curve_detection(self, tracking_lead, v_ego, frogpilot_toggles):
+    if v_ego > CRUISING_SPEED:
+      curve_active = self.curve_detected and (0.9 / self.frogpilot_planner.road_curvature)**0.5 < v_ego
+
+      self.curvature_filter.update(self.frogpilot_planner.road_curvature_detected or curve_active)
+      self.curve_detected = self.curvature_filter.x >= THRESHOLD
+    else:
+      self.curvature_filter.x = 0
+      self.curve_detected = False
+
+  def slow_lead(self, tracking_lead, v_lead, frogpilot_toggles):
+    if tracking_lead:
+      slower_lead = frogpilot_toggles.conditional_slower_lead and self.frogpilot_planner.frogpilot_following.slower_lead
+      stopped_lead = frogpilot_toggles.conditional_stopped_lead and v_lead < 1
+
+      self.slow_lead_detected = self.slow_lead_filter.update(1 if slower_lead or stopped_lead else 0) >= THRESHOLD
+    else:
+      self.slow_lead_filter.update(0)
+      self.slow_lead_detected = False
+
+  def stop_sign_and_light(self, frogpilotCarState, tracking_lead, v_ego, frogpilot_toggles):
+    if not (self.curve_detected or tracking_lead or frogpilotCarState.trafficModeActive):
+      model_stopping = self.frogpilot_planner.model_length < v_ego * frogpilot_toggles.conditional_model_stop_time
+
+      self.stop_light_filter.update(self.frogpilot_planner.model_stopped or model_stopping)
+      self.stop_light_detected = self.stop_light_filter.x >= THRESHOLD**2
+    else:
+      self.stop_light_filter.x = 0
+      self.stop_light_detected = False

selfdrive/frogpilot/controls/lib/frogpilot_acceleration.py

@@ -0,0 +1,83 @@
+#!/usr/bin/env python3
+from openpilot.common.numpy_fast import clip, interp
+
+from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, get_max_accel
+
+from openpilot.selfdrive.frogpilot.frogpilot_variables import CITY_SPEED_LIMIT
+
+A_CRUISE_MIN_ECO =   A_CRUISE_MIN / 2
+A_CRUISE_MIN_SPORT = A_CRUISE_MIN * 2
+
+                       # MPH = [0.0,  11,  22,  34,  45,  56,  89]
+A_CRUISE_MAX_BP_CUSTOM =       [0.0,  5., 10., 15., 20., 25., 40.]
+A_CRUISE_MAX_VALS_ECO =        [2.0, 1.5, 1.0, 0.8, 0.6, 0.4, 0.2]
+A_CRUISE_MAX_VALS_SPORT =      [3.0, 2.5, 2.0, 1.5, 1.0, 0.8, 0.6]
+A_CRUISE_MAX_VALS_SPORT_PLUS = [4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0]
+
+def get_max_accel_eco(v_ego):
+  return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_ECO)
+
+def get_max_accel_sport(v_ego):
+  return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT)
+
+def get_max_accel_sport_plus(v_ego):
+  return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT_PLUS)
+
+def get_max_accel_low_speeds(max_accel, v_cruise):
+  return interp(v_cruise, [0., CITY_SPEED_LIMIT / 2, CITY_SPEED_LIMIT], [max_accel / 4, max_accel / 2, max_accel])
+
+def get_max_accel_ramp_off(max_accel, v_cruise, v_ego):
+  return interp(v_cruise - v_ego, [0., 1., 5., 10.], [0., 0.5, 1.0, max_accel])
+
+def get_max_allowed_accel(v_ego):
+  return interp(v_ego, [0., 5., 20.], [4.0, 4.0, 2.0])  # ISO 15622:2018
+
+class FrogPilotAcceleration:
+  def __init__(self, FrogPilotPlanner):
+    self.frogpilot_planner = FrogPilotPlanner
+
+    self.max_accel = 0
+    self.min_accel = 0
+
+  def update(self, frogpilotCarState, v_ego, frogpilot_toggles):
+    eco_gear = frogpilotCarState.ecoGear
+    sport_gear = frogpilotCarState.sportGear
+
+    if frogpilotCarState.trafficModeActive:
+      self.max_accel = get_max_accel(v_ego)
+    elif frogpilot_toggles.map_acceleration and (eco_gear or sport_gear):
+      if eco_gear:
+        self.max_accel = get_max_accel_eco(v_ego)
+      else:
+        if frogpilot_toggles.acceleration_profile == 3:
+          self.max_accel = get_max_accel_sport_plus(v_ego)
+        else:
+          self.max_accel = get_max_accel_sport(v_ego)
+    else:
+      if frogpilot_toggles.acceleration_profile == 1:
+        self.max_accel = get_max_accel_eco(v_ego)
+      elif frogpilot_toggles.acceleration_profile == 2:
+        self.max_accel = get_max_accel_sport(v_ego)
+      elif frogpilot_toggles.acceleration_profile == 3:
+        self.max_accel = get_max_accel_sport_plus(v_ego)
+      else:
+        self.max_accel = get_max_accel(v_ego)
+
+    if frogpilot_toggles.human_acceleration:
+      if self.frogpilot_planner.frogpilot_following.following_lead and not frogpilotCarState.trafficModeActive:
+        self.max_accel = clip(self.frogpilot_planner.lead_one.aLeadK, get_max_accel_sport_plus(v_ego), get_max_allowed_accel(v_ego))
+      self.max_accel = min(get_max_accel_low_speeds(self.max_accel, self.frogpilot_planner.v_cruise), self.max_accel)
+      self.max_accel = min(get_max_accel_ramp_off(self.max_accel, self.frogpilot_planner.v_cruise, v_ego), self.max_accel)
+
+    if frogpilot_toggles.map_deceleration and (eco_gear or sport_gear):
+      if eco_gear:
+        self.min_accel = A_CRUISE_MIN_ECO
+      else:
+        self.min_accel = A_CRUISE_MIN_SPORT
+    else:
+      if frogpilot_toggles.deceleration_profile == 1:
+        self.min_accel = A_CRUISE_MIN_ECO
+      elif frogpilot_toggles.deceleration_profile == 2:
+        self.min_accel = A_CRUISE_MIN_SPORT
+      else:
+        self.min_accel = A_CRUISE_MIN

selfdrive/frogpilot/controls/lib/frogpilot_events.py

@@ -0,0 +1,202 @@
+#!/usr/bin/env python3
+import random
+
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.controls.controlsd import Desire
+from openpilot.selfdrive.controls.lib.events import EventName, Events
+
+from openpilot.selfdrive.frogpilot.assets.theme_manager import update_wheel_image
+from openpilot.selfdrive.frogpilot.frogpilot_variables import CRUISING_SPEED, params, params_memory
+
+RANDOM_EVENTS_CHANCE = 0.01 * DT_MDL
+
+class FrogPilotEvents:
+  def __init__(self, FrogPilotPlanner):
+    self.frogpilot_planner = FrogPilotPlanner
+
+    self.events = Events()
+
+    self.accel30_played = False
+    self.accel35_played = False
+    self.accel40_played = False
+    self.always_on_lateral_active_previously = False
+    self.dejaVu_played = False
+    self.fcw_played = False
+    self.firefox_played = False
+    self.goat_played = False
+    self.holiday_theme_played = False
+    self.no_entry_alert_played = False
+    self.openpilot_crashed_played = False
+    self.previous_traffic_mode = False
+    self.random_event_played = False
+    self.stopped_for_light = False
+    self.this_is_fine_played = False
+    self.vCruise69_played = False
+    self.youveGotMail_played = False
+
+    self.frame = 0
+    self.holiday_theme_frame = 0
+    self.max_acceleration = 0
+    self.random_event_timer = 0
+    self.tracking_lead_distance = 0
+
+  def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState, lead_distance, modelData, v_lead, frogpilot_toggles):
+    self.events.clear()
+
+    if self.random_event_played:
+      self.random_event_timer += DT_MDL
+      if self.random_event_timer >= 4:
+        update_wheel_image(frogpilot_toggles.wheel_image, frogpilot_toggles.current_holiday_theme, False)
+        params_memory.put_bool("UpdateWheelImage", True)
+        self.random_event_played = False
+        self.random_event_timer = 0
+
+    if self.frogpilot_planner.frogpilot_vcruise.forcing_stop:
+      self.events.add(EventName.forcingStop)
+
+    if frogpilot_toggles.green_light_alert and not self.frogpilot_planner.tracking_lead and carState.standstill:
+      if not self.frogpilot_planner.model_stopped and self.stopped_for_light:
+        self.events.add(EventName.greenLight)
+      self.stopped_for_light = self.frogpilot_planner.cem.stop_light_detected
+    else:
+      self.stopped_for_light = False
+
+    if not self.holiday_theme_played and frogpilot_toggles.current_holiday_theme != "stock" and self.frame >= 10:
+      if self.holiday_theme_frame >= 1:
+        self.events.add(EventName.holidayActive)
+        self.holiday_theme_played = True
+      self.holiday_theme_frame += DT_MDL
+
+    if frogpilot_toggles.lead_departing_alert and self.frogpilot_planner.tracking_lead and carState.standstill:
+      if self.tracking_lead_distance == 0:
+        self.tracking_lead_distance = lead_distance
+
+      lead_departing = lead_distance - self.tracking_lead_distance > 1
+      lead_departing &= v_lead > 1
+
+      if lead_departing:
+        self.events.add(EventName.leadDeparting)
+    else:
+      self.tracking_lead_distance = 0
+
+    if not self.openpilot_crashed_played and self.frogpilot_planner.error_log.is_file():
+      if frogpilot_toggles.random_events:
+        self.events.add(EventName.openpilotCrashedRandomEvent)
+      else:
+        self.events.add(EventName.openpilotCrashed)
+      self.openpilot_crashed_played = True
+
+    if not self.random_event_played and frogpilot_toggles.random_events:
+      acceleration = carState.aEgo
+
+      if not carState.gasPressed:
+        self.max_acceleration = max(acceleration, self.max_acceleration)
+      else:
+        self.max_acceleration = 0
+
+      if not self.accel30_played and 3.5 > self.max_acceleration >= 3.0 and acceleration < 1.5:
+        self.events.add(EventName.accel30)
+        update_wheel_image("weeb_wheel")
+        params_memory.put_bool("UpdateWheelImage", True)
+        self.accel30_played = True
+        self.random_event_played = True
+        self.max_acceleration = 0
+
+      elif not self.accel35_played and 4.0 > self.max_acceleration >= 3.5 and acceleration < 1.5:
+        self.events.add(EventName.accel35)
+        update_wheel_image("tree_fiddy")
+        params_memory.put_bool("UpdateWheelImage", True)
+        self.accel35_played = True
+        self.random_event_played = True
+        self.max_acceleration = 0
+
+      elif not self.accel40_played and self.max_acceleration >= 4.0 and acceleration < 1.5:
+        self.events.add(EventName.accel40)
+        update_wheel_image("great_scott")
+        params_memory.put_bool("UpdateWheelImage", True)
+        self.accel40_played = True
+        self.random_event_played = True
+        self.max_acceleration = 0
+
+      if not self.dejaVu_played and carState.vEgo > CRUISING_SPEED * 2:
+        if carState.vEgo > (1 / self.frogpilot_planner.road_curvature)**0.75 * 2 > CRUISING_SPEED * 2 and abs(carState.steeringAngleDeg) > 30:
+          self.events.add(EventName.dejaVuCurve)
+          self.dejaVu_played = True
+          self.random_event_played = True
+
+      if not self.no_entry_alert_played and frogpilotCarControl.noEntryEventTriggered:
+        self.events.add(EventName.hal9000)
+        self.no_entry_alert_played = True
+        self.random_event_played = True
+
+      if frogpilotCarControl.steerSaturatedEventTriggered:
+        event_choices = []
+        if not self.firefox_played:
+          event_choices.append("firefoxSteerSaturated")
+        if not self.goat_played:
+          event_choices.append("goatSteerSaturated")
+        if not self.this_is_fine_played:
+          event_choices.append("thisIsFineSteerSaturated")
+
+        if random.random() < RANDOM_EVENTS_CHANCE and event_choices:
+          event_choice = random.choice(event_choices)
+          if event_choice == "firefoxSteerSaturated":
+            self.events.add(EventName.firefoxSteerSaturated)
+            update_wheel_image("firefox")
+            params_memory.put_bool("UpdateWheelImage", True)
+            self.firefox_played = True
+          elif event_choice == "goatSteerSaturated":
+            self.events.add(EventName.goatSteerSaturated)
+            update_wheel_image("goat")
+            params_memory.put_bool("UpdateWheelImage", True)
+            self.goat_played = True
+          elif event_choice == "thisIsFineSteerSaturated":
+            self.events.add(EventName.thisIsFineSteerSaturated)
+            update_wheel_image("this_is_fine")
+            params_memory.put_bool("UpdateWheelImage", True)
+            self.this_is_fine_played = True
+          self.random_event_played = True
+
+      if not self.vCruise69_played and 70 > max(controlsState.vCruise, controlsState.vCruiseCluster) * (1 if frogpilot_toggles.is_metric else CV.KPH_TO_MPH) >= 69:
+        self.events.add(EventName.vCruise69)
+        self.vCruise69_played = True
+        self.random_event_played = True
+
+      if not self.fcw_played and frogpilotCarControl.fcwEventTriggered:
+        event_choices = ["toBeContinued", "yourFrogTriedToKillMe"]
+        if random.random() < RANDOM_EVENTS_CHANCE:
+          event_choice = random.choice(event_choices)
+          if event_choice == "toBeContinued":
+            self.events.add(EventName.toBeContinued)
+          elif event_choice == "yourFrogTriedToKillMe":
+            self.events.add(EventName.yourFrogTriedToKillMe)
+        self.fcw_played = True
+        self.random_event_played = True
+
+      if not self.youveGotMail_played and frogpilotCarControl.alwaysOnLateralActive and not self.always_on_lateral_active_previously:
+        if random.random() < RANDOM_EVENTS_CHANCE and not carState.standstill:
+          self.events.add(EventName.youveGotMail)
+          self.youveGotMail_played = True
+          self.random_event_played = True
+      self.always_on_lateral_active_previously = frogpilotCarControl.alwaysOnLateralActive
+
+    if frogpilot_toggles.speed_limit_changed_alert and self.frogpilot_planner.frogpilot_vcruise.slc.speed_limit_changed and self.frogpilot_planner.frogpilot_vcruise.speed_limit_timer < 1:
+      self.events.add(EventName.speedLimitChanged)
+
+    if 5 > self.frame > 4 and params.get("NNFFModelName", encoding='utf-8') is not None:
+      self.events.add(EventName.torqueNNLoad)
+
+    if frogpilotCarState.trafficModeActive != self.previous_traffic_mode:
+      if self.previous_traffic_mode:
+        self.events.add(EventName.trafficModeInactive)
+      else:
+        self.events.add(EventName.trafficModeActive)
+      self.previous_traffic_mode = frogpilotCarState.trafficModeActive
+
+    if modelData.meta.turnDirection == Desire.turnLeft:
+      self.events.add(EventName.turningLeft)
+    elif modelData.meta.turnDirection == Desire.turnRight:
+      self.events.add(EventName.turningRight)
+
+    self.frame += DT_MDL

selfdrive/frogpilot/controls/lib/frogpilot_following.py

@@ -0,0 +1,87 @@
+#!/usr/bin/env python3
+from openpilot.common.numpy_fast import clip, interp
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, STOP_DISTANCE, desired_follow_distance, get_jerk_factor, get_T_FOLLOW
+
+from openpilot.selfdrive.frogpilot.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED
+
+TRAFFIC_MODE_BP = [0., CITY_SPEED_LIMIT]
+
+class FrogPilotFollowing:
+  def __init__(self, FrogPilotPlanner):
+    self.frogpilot_planner = FrogPilotPlanner
+
+    self.following_lead = False
+    self.slower_lead = False
+
+    self.acceleration_jerk = 0
+    self.base_acceleration_jerk = 0
+    self.base_speed_jerk = 0
+    self.danger_jerk = 0
+    self.desired_follow_distance = 0
+    self.speed_jerk = 0
+    self.t_follow = 0
+
+  def update(self, aEgo, controlsState, frogpilotCarState, lead_distance, v_ego, v_lead, frogpilot_toggles):
+    if frogpilotCarState.trafficModeActive:
+      if aEgo >= 0:
+        self.base_acceleration_jerk = interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_jerk_acceleration)
+        self.base_speed_jerk = interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_jerk_speed)
+      else:
+        self.base_acceleration_jerk = interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_jerk_deceleration)
+        self.base_speed_jerk = interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_jerk_speed_decrease)
+
+      self.base_danger_jerk = interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_jerk_danger)
+      self.t_follow = interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_follow)
+    else:
+      if aEgo >= 0:
+        self.base_acceleration_jerk, self.base_danger_jerk, self.base_speed_jerk = get_jerk_factor(
+          frogpilot_toggles.aggressive_jerk_acceleration, frogpilot_toggles.aggressive_jerk_danger, frogpilot_toggles.aggressive_jerk_speed,
+          frogpilot_toggles.standard_jerk_acceleration, frogpilot_toggles.standard_jerk_danger, frogpilot_toggles.standard_jerk_speed,
+          frogpilot_toggles.relaxed_jerk_acceleration, frogpilot_toggles.relaxed_jerk_danger, frogpilot_toggles.relaxed_jerk_speed,
+          frogpilot_toggles.custom_personalities, controlsState.personality
+        )
+      else:
+        self.base_acceleration_jerk, self.base_danger_jerk, self.base_speed_jerk = get_jerk_factor(
+          frogpilot_toggles.aggressive_jerk_deceleration, frogpilot_toggles.aggressive_jerk_danger, frogpilot_toggles.aggressive_jerk_speed_decrease,
+          frogpilot_toggles.standard_jerk_deceleration, frogpilot_toggles.standard_jerk_danger, frogpilot_toggles.standard_jerk_speed_decrease,
+          frogpilot_toggles.relaxed_jerk_deceleration, frogpilot_toggles.relaxed_jerk_danger, frogpilot_toggles.relaxed_jerk_speed_decrease,
+          frogpilot_toggles.custom_personalities, controlsState.personality
+        )
+
+      self.t_follow = get_T_FOLLOW(
+        frogpilot_toggles.aggressive_follow,
+        frogpilot_toggles.standard_follow,
+        frogpilot_toggles.relaxed_follow,
+        frogpilot_toggles.custom_personalities, controlsState.personality
+      )
+
+    self.acceleration_jerk = self.base_acceleration_jerk
+    self.danger_jerk = self.base_danger_jerk
+    self.speed_jerk = self.base_speed_jerk
+
+    self.following_lead = self.frogpilot_planner.tracking_lead and lead_distance < (self.t_follow + 1) * v_ego
+
+    if self.frogpilot_planner.tracking_lead:
+      self.update_follow_values(lead_distance, v_ego, v_lead, frogpilot_toggles)
+      self.desired_follow_distance = int(desired_follow_distance(v_ego, v_lead, self.t_follow))
+    else:
+      self.desired_follow_distance = 0
+
+  def update_follow_values(self, lead_distance, v_ego, v_lead, frogpilot_toggles):
+    # Offset by FrogAi for FrogPilot for a more natural approach to a faster lead
+    if frogpilot_toggles.human_following and v_lead > v_ego:
+      distance_factor = max(lead_distance - (v_ego * self.t_follow), 1)
+      standstill_offset = max(STOP_DISTANCE - v_ego, 1)
+      acceleration_offset = clip((v_lead - v_ego) * standstill_offset - COMFORT_BRAKE, 1, distance_factor)
+      self.acceleration_jerk /= acceleration_offset
+      self.speed_jerk /= acceleration_offset
+      self.t_follow /= acceleration_offset
+
+    # Offset by FrogAi for FrogPilot for a more natural approach to a slower lead
+    if (frogpilot_toggles.conditional_slower_lead or frogpilot_toggles.human_following) and v_lead < v_ego > CRUISING_SPEED:
+      distance_factor = max(lead_distance - (v_lead * self.t_follow), 1)
+      far_lead_offset = max(v_lead - CITY_SPEED_LIMIT, 1)
+      braking_offset = clip(min(v_ego - v_lead, v_lead) * far_lead_offset - COMFORT_BRAKE, 1, distance_factor)
+      if frogpilot_toggles.human_following:
+        self.t_follow /= braking_offset
+      self.slower_lead = braking_offset / far_lead_offset > 1

selfdrive/frogpilot/controls/lib/frogpilot_tracking.py

@@ -0,0 +1,37 @@
+#!/usr/bin/env python3
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+
+class FrogPilotTracking:
+  def __init__(self):
+    self.params_tracking = Params("/persist/tracking")
+
+    self.total_drives = self.params_tracking.get_int("FrogPilotDrives")
+    self.total_kilometers = self.params_tracking.get_float("FrogPilotKilometers")
+    self.total_minutes = self.params_tracking.get_float("FrogPilotMinutes")
+
+    self.drive_added = False
+    self.enabled = False
+
+    self.drive_distance = 0
+    self.drive_time = 0
+
+  def update(self, carState, controlsState, frogpilotCarControl):
+    self.enabled |= controlsState.enabled or frogpilotCarControl.alwaysOnLateralActive
+
+    self.drive_distance += carState.vEgo * DT_MDL
+    self.drive_time += DT_MDL
+
+    if self.drive_time > 60 and carState.standstill and self.enabled:
+      self.total_kilometers += self.drive_distance / 1000
+      self.params_tracking.put_float_nonblocking("FrogPilotKilometers", self.total_kilometers)
+      self.drive_distance = 0
+
+      self.total_minutes += self.drive_time / 60
+      self.params_tracking.put_float_nonblocking("FrogPilotMinutes", self.total_minutes)
+      self.drive_time = 0
+
+      if not self.drive_added:
+        self.total_drives += 1
+        self.params_tracking.put_int_nonblocking("FrogPilotDrives", self.total_drives)
+        self.drive_added = True

selfdrive/frogpilot/controls/lib/frogpilot_vcruise.py

@@ -0,0 +1,166 @@
+#!/usr/bin/env python3
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.numpy_fast import clip
+from openpilot.common.realtime import DT_MDL
+
+from openpilot.selfdrive.controls.controlsd import ButtonType
+from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_UNSET
+
+from openpilot.selfdrive.frogpilot.controls.lib.map_turn_speed_controller import MapTurnSpeedController
+from openpilot.selfdrive.frogpilot.controls.lib.smart_turn_speed_controller import SmartTurnSpeedController
+from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController
+from openpilot.selfdrive.frogpilot.frogpilot_variables import CRUISING_SPEED, PLANNER_TIME, params_memory
+
+TARGET_LAT_A = 2.0
+
+class FrogPilotVCruise:
+  def __init__(self, FrogPilotPlanner):
+    self.frogpilot_planner = FrogPilotPlanner
+
+    self.mtsc = MapTurnSpeedController()
+    self.slc = SpeedLimitController()
+    self.stsc = SmartTurnSpeedController(self)
+
+    self.forcing_stop = False
+    self.override_force_stop = False
+    self.override_slc = False
+
+    self.force_stop_timer = 0
+    self.mtsc_target = 0
+    self.overridden_speed = 0
+    self.override_force_stop_timer = 0
+    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
+
+  def update(self, carControl, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, v_cruise, v_ego, frogpilot_toggles):
+    force_stop = frogpilot_toggles.force_stops and self.frogpilot_planner.cem.stop_light_detected and controlsState.enabled
+    force_stop &= self.frogpilot_planner.model_length < 100
+    force_stop &= self.override_force_stop_timer <= 0
+
+    self.force_stop_timer = self.force_stop_timer + DT_MDL if force_stop else 0
+
+    force_stop_enabled = self.force_stop_timer >= 1
+
+    self.override_force_stop |= not frogpilot_toggles.force_standstill and carState.standstill and self.frogpilot_planner.tracking_lead
+    self.override_force_stop |= carState.gasPressed
+    self.override_force_stop |= frogpilotCarControl.accelPressed
+    self.override_force_stop &= force_stop_enabled
+
+    if self.override_force_stop:
+      self.override_force_stop_timer = 10
+    elif self.override_force_stop_timer > 0:
+      self.override_force_stop_timer -= DT_MDL
+
+    v_cruise_cluster = max(controlsState.vCruiseCluster * CV.KPH_TO_MS, v_cruise)
+    v_cruise_diff = v_cruise_cluster - v_cruise
+
+    v_ego_cluster = max(carState.vEgoCluster, v_ego)
+    v_ego_diff = v_ego_cluster - v_ego
+
+    # 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:
+      mtsc_active = self.mtsc_target < v_cruise
+      mtsc_speed = ((TARGET_LAT_A * frogpilot_toggles.turn_aggressiveness) / (self.mtsc.get_map_curvature(v_ego) * frogpilot_toggles.curve_sensitivity))**0.5
+      self.mtsc_target = clip(mtsc_speed, CRUISING_SPEED, v_cruise)
+
+      if self.frogpilot_planner.road_curvature_detected and mtsc_active:
+        self.mtsc_target = self.frogpilot_planner.v_cruise
+      elif not self.frogpilot_planner.road_curvature_detected and frogpilot_toggles.mtsc_curvature_check:
+        self.mtsc_target = v_cruise
+    else:
+      self.mtsc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0
+
+    # Pfeiferj's Speed Limit Controller
+    if frogpilot_toggles.show_speed_limits or frogpilot_toggles.speed_limit_controller:
+      self.slc.update(frogpilotCarState.dashboardSpeedLimit, controlsState.enabled, frogpilotNavigation.navigationSpeedLimit, v_cruise_cluster, v_ego, frogpilot_toggles)
+      desired_slc_target = self.slc.desired_speed_limit
+
+      if self.slc.speed_limit_changed:
+        speed_limit_accepted = frogpilotCarControl.accelPressed and carControl.longActive or params_memory.get_bool("SpeedLimitAccepted")
+        speed_limit_denied = frogpilotCarControl.decelPressed and carControl.longActive or self.speed_limit_timer >= 30
+
+        if speed_limit_accepted:
+          self.slc_target = desired_slc_target
+          params_memory.remove("SpeedLimitAccepted")
+        elif desired_slc_target < self.slc_target and not frogpilot_toggles.speed_limit_confirmation_lower:
+          self.slc_target = desired_slc_target
+        elif desired_slc_target > self.slc_target and not frogpilot_toggles.speed_limit_confirmation_higher:
+          self.slc_target = desired_slc_target
+        else:
+          self.speed_limit_timer += DT_MDL
+
+        self.slc.speed_limit_changed = self.slc_target != desired_slc_target and not speed_limit_denied
+      elif self.slc_target == 0:
+        self.slc_target = desired_slc_target
+      else:
+        self.speed_limit_timer = 0
+
+      if frogpilot_toggles.speed_limit_controller:
+        self.override_slc = self.overridden_speed > self.slc_target + self.slc_offset
+        self.override_slc |= carState.gasPressed and v_ego > self.slc_target + self.slc_offset
+        self.override_slc &= controlsState.enabled
+
+        if self.override_slc:
+          if frogpilot_toggles.speed_limit_controller_override_manual:
+            if carState.gasPressed:
+              self.overridden_speed = v_ego_cluster
+            self.overridden_speed = clip(self.overridden_speed, self.slc_target + self.slc_offset, v_cruise_cluster)
+          elif frogpilot_toggles.speed_limit_controller_override_set_speed:
+            self.overridden_speed = v_cruise_cluster
+        else:
+          self.overridden_speed = 0
+      else:
+        self.override_slc = False
+        self.overridden_speed = 0
+
+      self.slc_offset = self.slc.get_offset(self.slc_target, frogpilot_toggles)
+    else:
+      self.slc_offset = 0
+      self.slc_target = 0
+
+    # Pfeiferj's Vision Turn Controller
+    if frogpilot_toggles.vision_turn_speed_controller and v_ego > CRUISING_SPEED and carControl.longActive and self.frogpilot_planner.road_curvature_detected:
+      self.vtsc_target = ((TARGET_LAT_A * frogpilot_toggles.turn_aggressiveness) / (self.frogpilot_planner.road_curvature * frogpilot_toggles.curve_sensitivity))**0.5
+      self.vtsc_target = clip(self.vtsc_target, CRUISING_SPEED, v_cruise)
+    else:
+      self.vtsc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0
+
+    if frogpilot_toggles.force_standstill and carState.standstill and not self.override_force_stop and controlsState.enabled:
+      self.forcing_stop = True
+      v_cruise = -1
+
+    elif force_stop_enabled and not self.override_force_stop:
+      self.forcing_stop |= not carState.standstill
+      self.tracked_model_length = max(self.tracked_model_length - v_ego * DT_MDL, 0)
+      v_cruise = min((self.tracked_model_length // PLANNER_TIME), v_cruise)
+
+    else:
+      if not self.frogpilot_planner.cem.stop_light_detected:
+        self.override_force_stop = False
+
+      self.forcing_stop = False
+
+      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.stsc_target, self.vtsc_target]
+      else:
+        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

selfdrive/frogpilot/controls/lib/map_turn_speed_controller.py

@@ -0,0 +1,81 @@
+# PFEIFER - MTSC - Modified by FrogAi for FrogPilot
+#!/usr/bin/env python3
+import json
+import math
+
+from openpilot.selfdrive.frogpilot.frogpilot_utilities import calculate_distance_to_point
+from openpilot.selfdrive.frogpilot.frogpilot_variables import PLANNER_TIME, TO_RADIANS, params_memory
+
+def calculate_curvature(p1, p2, p3):
+  lat1, lon1 = p1
+  lat2, lon2 = p2
+  lat3, lon3 = p3
+
+  lat1_rad, lon1_rad = lat1 * TO_RADIANS, lon1 * TO_RADIANS
+  lat2_rad, lon2_rad = lat2 * TO_RADIANS, lon2 * TO_RADIANS
+  lat3_rad, lon3_rad = lat3 * TO_RADIANS, lon3 * TO_RADIANS
+
+  side_a = calculate_distance_to_point(lat2_rad, lon2_rad, lat3_rad, lon3_rad)
+  side_b = calculate_distance_to_point(lat1_rad, lon1_rad, lat3_rad, lon3_rad)
+  side_c = calculate_distance_to_point(lat1_rad, lon1_rad, lat2_rad, lon2_rad)
+
+  s = (side_a + side_b + side_c) / 2
+
+  area_squared = s * (s - side_a) * (s - side_b) * (s - side_c)
+  if area_squared <= 0:
+    return 0
+
+  area = math.sqrt(area_squared)
+
+  radius = (side_a * side_b * side_c) / (4 * area)
+  if radius == 0:
+    return 0
+
+  curvature = 1 / radius
+  return curvature
+
+class MapTurnSpeedController:
+  def get_map_curvature(self, v_ego):
+    position = json.loads(params_memory.get("LastGPSPosition") or "{}")
+    if not position:
+      return 1e-6
+    current_latitude = position["latitude"]
+    current_longitude = position["longitude"]
+
+    distances = []
+    minimum_idx = 0
+    minimum_distance = 1000.0
+
+    target_velocities = json.loads(params_memory.get("MapTargetVelocities") or "[]")
+    for i, target_velocity in enumerate(target_velocities):
+      target_latitude = target_velocity["latitude"]
+      target_longitude = target_velocity["longitude"]
+
+      distance = calculate_distance_to_point(current_latitude * TO_RADIANS, current_longitude * TO_RADIANS, target_latitude * TO_RADIANS, target_longitude * TO_RADIANS)
+      distances.append(distance)
+
+      if distance < minimum_distance:
+        minimum_distance = distance
+        minimum_idx = i
+
+    forward_distances = distances[minimum_idx:]
+
+    cumulative_distance = 0.0
+    target_idx = None
+
+    for i, distance in enumerate(forward_distances):
+      cumulative_distance += distance
+      if cumulative_distance >= PLANNER_TIME * v_ego:
+        target_idx = i
+        break
+
+    forward_points = target_velocities[minimum_idx:]
+
+    if target_idx is None or target_idx == 0 or target_idx >= len(forward_points) - 1:
+      return 1e-6
+
+    p1 = (forward_points[target_idx - 1]["latitude"], forward_points[target_idx - 1]["longitude"])
+    p2 = (forward_points[target_idx]["latitude"], forward_points[target_idx]["longitude"])
+    p3 = (forward_points[target_idx + 1]["latitude"], forward_points[target_idx + 1]["longitude"])
+
+    return max(calculate_curvature(p1, p2, p3), 1e-6)

selfdrive/frogpilot/controls/lib/smart_turn_speed_controller.py

@@ -0,0 +1,88 @@
+#!/usr/bin/env python3
+import json
+import numpy as np
+
+from sortedcontainers import SortedDict
+
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.numpy_fast import clip
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
+
+from openpilot.selfdrive.frogpilot.frogpilot_variables import CRUISING_SPEED, PLANNER_TIME, params
+
+CACHE_WINDOW = 1
+CURVATURE_THRESHOLD = 1e-10
+ROUNDING_PRECISION = 10
+
+class SmartTurnSpeedController:
+  def __init__(self, FrogPilotVCruise):
+    self.frogpilot_vcruise = 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 = 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)
+    lateral_accel = round(v_ego**2 * road_curvature, ROUNDING_PRECISION)
+
+    if abs(road_curvature) < CURVATURE_THRESHOLD or abs(lateral_accel) < 1:
+      return
+
+    entries = self.data.setdefault(v_ego, [])
+    for i, entry in enumerate(entries):
+      if abs(entry[0] - road_curvature) < CURVATURE_THRESHOLD:
+        entries[i][1] = (entry[1] + lateral_accel) / 2
+        return
+
+    entries.append(np.array([road_curvature, lateral_accel]))
+
+  def update(self, carControl, v_cruise, v_ego):
+    if not carControl.longActive and V_CRUISE_MAX * CV.KPH_TO_MS >= v_ego > CRUISING_SPEED and not self.frogpilot_vcruise.frogpilot_planner.tracking_lead:
+      if self.manual_long_timer >= PLANNER_TIME:
+        self.update_curvature_data(v_ego)
+      self.manual_long_timer += DT_MDL
+
+    elif self.manual_long_timer >= PLANNER_TIME:
+      params.put_nonblocking("UserCurvature", json.dumps([
+        {"speed": speed, "curvatures": [{"curvature": entry[0], "lateral_accel": entry[1]} for entry in entries]}
+        for speed, entries in self.data.items()
+      ]))
+      self.manual_long_timer = 0
+
+    else:
+      self.set_stsc_target(carControl, v_cruise, v_ego)
+      self.manual_long_timer = 0

selfdrive/frogpilot/controls/lib/speed_limit_controller.py

@@ -0,0 +1,112 @@
+# PFEIFER - SLC - Modified by FrogAi for FrogPilot
+#!/usr/bin/env python3
+import json
+
+from openpilot.selfdrive.frogpilot.frogpilot_utilities import calculate_distance_to_point
+from openpilot.selfdrive.frogpilot.frogpilot_variables import TO_RADIANS, params, params_memory
+
+class SpeedLimitController:
+  def __init__(self):
+    self.experimental_mode = False
+    self.speed_limit_changed = False
+
+    self.desired_speed_limit = 0
+    self.map_speed_limit = 0
+    self.speed_limit = 0
+    self.upcoming_speed_limit = 0
+
+    self.source = "None"
+
+    self.previous_speed_limit = params.get_float("PreviousSpeedLimit")
+
+  def update(self, dashboard_speed_limit, enabled, navigation_speed_limit, v_cruise, v_ego, frogpilot_toggles):
+    self.update_map_speed_limit(v_ego, frogpilot_toggles)
+    max_speed_limit = v_cruise if enabled else 0
+
+    self.speed_limit = self.get_speed_limit(dashboard_speed_limit, max_speed_limit, navigation_speed_limit, frogpilot_toggles)
+    self.desired_speed_limit = self.get_desired_speed_limit()
+
+    self.experimental_mode = frogpilot_toggles.slc_fallback_experimental_mode and self.speed_limit == 0
+
+  def get_desired_speed_limit(self):
+    if self.speed_limit > 1:
+      if abs(self.speed_limit - self.previous_speed_limit) > 1:
+        params.put_float_nonblocking("PreviousSpeedLimit", self.speed_limit)
+        self.previous_speed_limit = self.speed_limit
+        self.speed_limit_changed = True
+      return self.speed_limit
+    else:
+      self.speed_limit_changed = False
+      return 0
+
+  def update_map_speed_limit(self, v_ego, frogpilot_toggles):
+    position = json.loads(params_memory.get("LastGPSPosition") or "{}")
+    if not position:
+      self.map_speed_limit = 0
+      return
+
+    self.map_speed_limit = params_memory.get_float("MapSpeedLimit")
+
+    next_map_speed_limit = json.loads(params_memory.get("NextMapSpeedLimit") or "{}")
+    self.upcoming_speed_limit = next_map_speed_limit.get("speedlimit", 0)
+    if self.upcoming_speed_limit > 1:
+      current_latitude = position.get("latitude")
+      current_longitude = position.get("longitude")
+
+      upcoming_latitude = next_map_speed_limit.get("latitude")
+      upcoming_longitude = next_map_speed_limit.get("longitude")
+
+      distance_to_upcoming = calculate_distance_to_point(current_latitude * TO_RADIANS, current_longitude * TO_RADIANS, upcoming_latitude * TO_RADIANS, upcoming_longitude * TO_RADIANS)
+
+      if self.previous_speed_limit < self.upcoming_speed_limit:
+        max_distance = frogpilot_toggles.map_speed_lookahead_higher * v_ego
+      else:
+        max_distance = frogpilot_toggles.map_speed_lookahead_lower * v_ego
+
+      if distance_to_upcoming < max_distance:
+        self.map_speed_limit = self.upcoming_speed_limit
+
+  def get_offset(self, speed_limit, frogpilot_toggles):
+    if speed_limit < 13.5:
+      return frogpilot_toggles.speed_limit_offset1
+    if speed_limit < 24:
+      return frogpilot_toggles.speed_limit_offset2
+    if speed_limit < 29:
+      return frogpilot_toggles.speed_limit_offset3
+    return frogpilot_toggles.speed_limit_offset4
+
+  def get_speed_limit(self, dashboard_speed_limit, max_speed_limit, navigation_speed_limit, frogpilot_toggles):
+    limits = {
+      "Dashboard": dashboard_speed_limit,
+      "Map Data": self.map_speed_limit,
+      "Navigation": navigation_speed_limit
+    }
+    filtered_limits = {source: float(limit) for source, limit in limits.items() if limit > 1}
+
+    if filtered_limits:
+      if frogpilot_toggles.speed_limit_priority_highest:
+        self.source = max(filtered_limits, key=filtered_limits.get)
+        return filtered_limits[self.source]
+
+      if frogpilot_toggles.speed_limit_priority_lowest:
+        self.source = min(filtered_limits, key=filtered_limits.get)
+        return filtered_limits[self.source]
+
+      for priority in [
+        frogpilot_toggles.speed_limit_priority1,
+        frogpilot_toggles.speed_limit_priority2,
+        frogpilot_toggles.speed_limit_priority3
+      ]:
+        if priority is not None and priority in filtered_limits:
+          self.source = priority
+          return filtered_limits[priority]
+
+    self.source = "None"
+
+    if frogpilot_toggles.slc_fallback_previous_speed_limit:
+      return self.previous_speed_limit
+
+    if frogpilot_toggles.slc_fallback_set_speed:
+      return max_speed_limit
+
+    return 0