wip

selfdrive/frogpilot/controls/frogpilot_planner.py

@@ -0,0 +1,325 @@
+import numpy as np
+
+import cereal.messaging as messaging
+
+from cereal import 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.desire_helper import LANE_CHANGE_SPEED_MIN
+from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
+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.system.version import get_short_branch
+
+from openpilot.selfdrive.frogpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode
+from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CITY_SPEED_LIMIT, CRUISING_SPEED, calculate_lane_width, calculate_road_curvature
+from openpilot.selfdrive.frogpilot.controls.lib.map_turn_speed_controller import MapTurnSpeedController
+from openpilot.selfdrive.frogpilot.controls.lib.model_manager import RADARLESS_MODELS
+from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController
+
+# Acceleration profiles - Credit goes to the DragonPilot team!
+                 # MPH = [0., 18,  36,  63,  94]
+A_CRUISE_MIN_BP_CUSTOM = [0., 8., 16., 28., 42.]
+                 # MPH = [0., 6.71, 13.4, 17.9, 24.6, 33.6, 44.7, 55.9, 67.1, 123]
+A_CRUISE_MAX_BP_CUSTOM = [0.,    3,   6.,   8.,  11.,  15.,  20.,  25.,  30., 55.]
+
+A_CRUISE_MIN_VALS_ECO = [-0.001, -0.010, -0.28, -0.56, -0.56]
+A_CRUISE_MAX_VALS_ECO = [3.5, 3.2, 2.3, 2.0, 1.15, .80, .58, .36, .30, .091]
+
+A_CRUISE_MIN_VALS_SPORT = [-0.50, -0.52, -0.55, -0.57, -0.60]
+A_CRUISE_MAX_VALS_SPORT = [3.5, 3.5, 3.3, 2.8, 1.5, 1.0, .75, .6, .38, .2]
+
+TRAFFIC_MODE_BP = [0., CITY_SPEED_LIMIT]
+
+TARGET_LAT_A = 1.9  # m/s^2
+
+def get_min_accel_eco(v_ego):
+  return interp(v_ego, A_CRUISE_MIN_BP_CUSTOM, A_CRUISE_MIN_VALS_ECO)
+
+def get_max_accel_eco(v_ego):
+  return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_ECO)
+
+def get_min_accel_sport(v_ego):
+  return interp(v_ego, A_CRUISE_MIN_BP_CUSTOM, A_CRUISE_MIN_VALS_SPORT)
+
+def get_max_accel_sport(v_ego):
+  return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT)
+
+class FrogPilotPlanner:
+  def __init__(self, CP):
+    self.CP = CP
+
+    self.params = Params()
+    self.params_memory = Params("/dev/shm/params")
+
+    self.cem = ConditionalExperimentalMode()
+    self.lead_one = Lead()
+    self.mtsc = MapTurnSpeedController()
+
+    self.release = get_short_branch() == "FrogPilot"
+
+    self.radarless_model = self.params.get("Model", encoding='utf-8') in RADARLESS_MODELS
+
+    self.override_slc = False
+
+    self.jerk = 0
+    self.overridden_speed = 0
+    self.mtsc_target = 0
+    self.slc_target = 0
+    self.t_follow = 0
+    self.vtsc_target = 0
+
+  def update(self, carState, controlsState, frogpilotCarControl, frogpilotNavigation, liveLocationKalman, modelData, radarState):
+    v_cruise_kph = min(controlsState.vCruise, V_CRUISE_MAX)
+    v_cruise = v_cruise_kph * CV.KPH_TO_MS
+    v_ego = max(carState.vEgo, 0)
+    v_lead = self.lead_one.vLead
+
+    if self.acceleration_profile == 1:
+      self.max_accel = get_max_accel_eco(v_ego)
+    elif self.acceleration_profile in (2, 3):
+      self.max_accel = get_max_accel_sport(v_ego)
+    elif not controlsState.experimentalMode:
+      self.max_accel = get_max_accel(v_ego)
+    else:
+      self.max_accel = ACCEL_MAX
+
+    v_cruise_changed = (self.mtsc_target or self.vtsc_target) < v_cruise
+
+    if self.deceleration_profile == 1 and not v_cruise_changed:
+      self.min_accel = get_min_accel_eco(v_ego)
+    elif self.deceleration_profile == 2 and not v_cruise_changed:
+      self.min_accel = get_min_accel_sport(v_ego)
+    elif not controlsState.experimentalMode:
+      self.min_accel = A_CRUISE_MIN
+    else:
+      self.min_accel = ACCEL_MIN
+
+    check_lane_width = self.adjacent_lanes or self.blind_spot_path or self.lane_detection
+    if check_lane_width and v_ego >= LANE_CHANGE_SPEED_MIN:
+      self.lane_width_left = float(calculate_lane_width(modelData.laneLines[0], modelData.laneLines[1], modelData.roadEdges[0]))
+      self.lane_width_right = float(calculate_lane_width(modelData.laneLines[3], modelData.laneLines[2], modelData.roadEdges[1]))
+    else:
+      self.lane_width_left = 0
+      self.lane_width_right = 0
+
+    road_curvature = calculate_road_curvature(modelData, v_ego)
+
+    if self.lead_one.status and self.CP.openpilotLongitudinalControl:
+      base_jerk = get_jerk_factor(self.custom_personalities, self.aggressive_jerk, self.standard_jerk, self.relaxed_jerk, controlsState.personality)
+      base_t_follow = get_T_FOLLOW(self.custom_personalities, self.aggressive_follow, self.standard_follow, self.relaxed_follow, controlsState.personality)
+      self.safe_obstacle_distance = int(np.mean(get_safe_obstacle_distance(v_ego, self.t_follow)))
+      self.safe_obstacle_distance_stock = int(np.mean(get_safe_obstacle_distance(v_ego, base_t_follow)))
+      self.stopped_equivalence_factor = int(np.mean(get_stopped_equivalence_factor(v_lead)))
+      self.jerk, self.t_follow = self.update_follow_values(base_jerk, self.lead_one, base_t_follow, frogpilotCarControl.trafficModeActive, v_ego, v_lead)
+    else:
+      self.safe_obstacle_distance = 0
+      self.safe_obstacle_distance_stock = 0
+      self.stopped_equivalence_factor = 0
+      self.t_follow = 1.45
+
+    self.v_cruise = self.update_v_cruise(carState, controlsState, controlsState.enabled, liveLocationKalman, modelData, road_curvature, v_cruise, v_ego)
+
+    if self.conditional_experimental_mode and self.CP.openpilotLongitudinalControl or self.green_light_alert:
+      self.cem.update(carState, controlsState.enabled, frogpilotNavigation, self.lead_one, modelData, road_curvature, self.t_follow, v_ego)
+
+    if self.radarless_model:
+      model_leads = list(modelData.leadsV3)
+      if len(model_leads) > 0:
+        model_lead = model_leads[0]
+        self.lead_one.update(model_lead.x[0], 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
+
+  def update_follow_values(self, jerk, lead_one, t_follow, trafficModeActive, v_ego, v_lead):
+    if trafficModeActive:
+      jerk = interp(v_ego, TRAFFIC_MODE_BP, self.traffic_mode_jerk)
+      t_follow = interp(v_ego, TRAFFIC_MODE_BP, self.traffic_mode_t_follow)
+
+    stopping_distance = STOP_DISTANCE + max(self.increased_stopping_distance - v_ego if not trafficModeActive else 0, 0)
+    lead_distance = self.lead_one.dRel + stopping_distance
+
+    # Offset by FrogAi for FrogPilot for a more natural takeoff with a lead
+    if self.aggressive_acceleration:
+      distance_factor = np.maximum(1, lead_distance - (v_ego * t_follow))
+      standstill_offset = max(stopping_distance - v_ego, 0)
+      acceleration_offset = np.clip((v_lead - v_ego) + standstill_offset - COMFORT_BRAKE, 1, distance_factor)
+      jerk /= acceleration_offset
+      t_follow /= acceleration_offset
+
+    # Offset by FrogAi for FrogPilot for a more natural approach to a slower lead
+    if self.smoother_braking:
+      distance_factor = np.maximum(1, lead_distance - (v_lead * t_follow))
+      far_lead_offset = max(lead_distance - CITY_SPEED_LIMIT, 0) if self.smoother_braking_far_lead else 0
+      braking_offset = np.clip((v_ego - v_lead) + far_lead_offset - COMFORT_BRAKE, 1, distance_factor)
+      if self.smoother_braking_jerk:
+        jerk *= max(braking_offset**(1 / COMFORT_BRAKE), 1)
+      t_follow /= braking_offset
+
+    return jerk, t_follow
+
+  def update_v_cruise(self, carState, controlsState, enabled, liveLocationKalman, modelData, road_curvature, v_cruise, v_ego):
+    gps_check = (liveLocationKalman.status == log.LiveLocationKalman.Status.valid) and liveLocationKalman.positionGeodetic.valid and liveLocationKalman.gpsOK
+
+    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
+
+    # Pfeiferj's Map Turn Speed Controller
+    if self.map_turn_speed_controller and v_ego > CRUISING_SPEED and enabled and gps_check:
+      mtsc_active = self.mtsc_target < v_cruise
+      self.mtsc_target = np.clip(self.mtsc.target_speed(v_ego, carState.aEgo), CRUISING_SPEED, v_cruise)
+
+      if self.mtsc_curvature_check and road_curvature < 1.0 and not mtsc_active:
+        self.mtsc_target = v_cruise
+      if self.mtsc_target == CRUISING_SPEED:
+        self.mtsc_target = v_cruise
+    else:
+      self.mtsc_target = v_cruise
+
+    # Pfeiferj's Speed Limit Controller
+    if self.speed_limit_controller:
+      SpeedLimitController.update(v_ego)
+      unconfirmed_slc_target = SpeedLimitController.desired_speed_limit
+
+      # Check if the new speed limit has been confirmed by the user
+      if self.speed_limit_confirmation:
+        if self.params_memory.get_bool("SLCConfirmed") or self.slc_target == 0:
+          self.slc_target = unconfirmed_slc_target
+          self.params_memory.put_bool("SLCConfirmed", False)
+      else:
+        self.slc_target = unconfirmed_slc_target
+
+      # Override SLC upon gas pedal press and reset upon brake/cancel button
+      self.override_slc &= self.overridden_speed > self.slc_target
+      self.override_slc |= carState.gasPressed and v_ego > self.slc_target
+      self.override_slc &= enabled
+
+      # Use the override speed if SLC is being overridden
+      if self.override_slc:
+        if self.speed_limit_controller_override == 1:
+          # Set the speed limit to the manual set speed
+          if carState.gasPressed:
+            self.overridden_speed = v_ego + v_ego_diff
+          self.overridden_speed = np.clip(self.overridden_speed, self.slc_target, v_cruise + v_cruise_diff)
+        elif self.speed_limit_controller_override == 2:
+          # Set the speed limit to the max set speed
+          self.overridden_speed = v_cruise + v_cruise_diff
+      else:
+        self.overridden_speed = 0
+    else:
+      self.slc_target = v_cruise
+
+    # Pfeiferj's Vision Turn Controller
+    if self.vision_turn_controller and v_ego > CRUISING_SPEED and enabled:
+      orientation_rate = np.array(np.abs(modelData.orientationRate.z)) * self.curve_sensitivity
+      velocity = np.array(modelData.velocity.x)
+
+      max_pred_lat_acc = np.amax(orientation_rate * velocity)
+      max_curve = max_pred_lat_acc / (v_ego**2)
+      adjusted_target_lat_a = TARGET_LAT_A * self.turn_aggressiveness
+
+      self.vtsc_target = (adjusted_target_lat_a / max_curve)**0.5
+      self.vtsc_target = np.clip(self.vtsc_target, CRUISING_SPEED, v_cruise)
+    else:
+      self.vtsc_target = v_cruise
+
+    targets = [self.mtsc_target, max(self.overridden_speed, self.slc_target) - v_ego_diff, self.vtsc_target]
+    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.jerk) if self.lead_one.status else 1)
+    frogpilotPlan.accelerationJerkStock = A_CHANGE_COST
+    frogpilotPlan.adjustedCruise = float(min(self.mtsc_target, self.vtsc_target) * (CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH))
+    frogpilotPlan.conditionalExperimental = self.cem.experimental_mode
+    frogpilotPlan.desiredFollowDistance = self.safe_obstacle_distance - self.stopped_equivalence_factor
+    frogpilotPlan.egoJerk = J_EGO_COST * (float(self.jerk) if self.lead_one.status else 1)
+    frogpilotPlan.egoJerkStock = J_EGO_COST
+    frogpilotPlan.jerk = float(self.jerk)
+    frogpilotPlan.safeObstacleDistance = self.safe_obstacle_distance
+    frogpilotPlan.safeObstacleDistanceStock = self.safe_obstacle_distance_stock
+    frogpilotPlan.stoppedEquivalenceFactor = self.stopped_equivalence_factor
+    frogpilotPlan.laneWidthLeft = self.lane_width_left
+    frogpilotPlan.laneWidthRight = self.lane_width_right
+    frogpilotPlan.minAcceleration = self.min_accel
+    frogpilotPlan.maxAcceleration = self.max_accel
+    frogpilotPlan.tFollow = float(self.t_follow)
+    frogpilotPlan.vCruise = float(self.v_cruise)
+
+    frogpilotPlan.redLight = self.cem.red_light_detected
+
+    frogpilotPlan.slcOverridden = bool(self.override_slc)
+    frogpilotPlan.slcOverriddenSpeed = float(self.overridden_speed)
+    frogpilotPlan.slcSpeedLimit = self.slc_target
+    frogpilotPlan.slcSpeedLimitOffset = SpeedLimitController.offset
+    frogpilotPlan.unconfirmedSlcSpeedLimit = SpeedLimitController.desired_speed_limit
+
+    frogpilotPlan.vtscControllingCurve = bool(self.mtsc_target > self.vtsc_target)
+
+    pm.send('frogpilotPlan', frogpilot_plan_send)
+
+  def update_frogpilot_params(self):
+    self.is_metric = self.params.get_bool("IsMetric")
+
+    self.conditional_experimental_mode = self.CP.openpilotLongitudinalControl and self.params.get_bool("ConditionalExperimental")
+    if self.conditional_experimental_mode:
+      self.cem.update_frogpilot_params()
+
+    custom_alerts = self.params.get_bool("CustomAlerts")
+    self.green_light_alert = custom_alerts and self.params.get_bool("GreenLightAlert")
+
+    self.custom_personalities = self.params.get_bool("CustomPersonalities")
+    self.aggressive_jerk = self.params.get_float("AggressiveJerk")
+    self.aggressive_follow = self.params.get_float("AggressiveFollow")
+    self.standard_jerk = self.params.get_float("StandardJerk")
+    self.standard_follow = self.params.get_float("StandardFollow")
+    self.relaxed_jerk = self.params.get_float("RelaxedJerk")
+    self.relaxed_follow = self.params.get_float("RelaxedFollow")
+    self.traffic_jerk = self.params.get_float("TrafficJerk")
+    self.traffic_follow = self.params.get_float("TrafficFollow")
+    self.traffic_mode_jerk = [self.traffic_jerk, self.aggressive_jerk] if self.custom_personalities and not self.release else [1.0, 0.5]
+    self.traffic_mode_t_follow = [self.traffic_follow, self.aggressive_follow] if self.custom_personalities and not self.release else [0.5, 1.0]
+
+    custom_ui = self.params.get_bool("CustomUI")
+    self.adjacent_lanes = custom_ui and self.params.get_bool("AdjacentPath")
+    self.blind_spot_path = custom_ui and self.params.get_bool("BlindSpotPath")
+
+    nudgeless_lane_change = self.params.get_bool("NudgelessLaneChange")
+    self.lane_detection = nudgeless_lane_change and self.params.get_int("LaneDetectionWidth") != 0
+
+    longitudinal_tune = self.CP.openpilotLongitudinalControl and self.params.get_bool("LongitudinalTune")
+    self.acceleration_profile = self.params.get_int("AccelerationProfile") if longitudinal_tune else 0
+    self.deceleration_profile = self.params.get_int("DecelerationProfile") if longitudinal_tune else 0
+    self.aggressive_acceleration = longitudinal_tune and self.params.get_bool("AggressiveAcceleration")
+    self.increased_stopping_distance = self.params.get_int("StoppingDistance") * (1 if self.is_metric else CV.FOOT_TO_METER) if longitudinal_tune else 0
+    self.smoother_braking = longitudinal_tune and self.params.get_bool("SmoothBraking")
+    self.smoother_braking_far_lead = self.smoother_braking and self.params.get_bool("SmoothBrakingFarLead") and not self.release
+    self.smoother_braking_jerk = self.smoother_braking and self.params.get_bool("SmoothBrakingJerk") and not self.release
+
+    self.map_turn_speed_controller = self.CP.openpilotLongitudinalControl and self.params.get_bool("MTSCEnabled")
+    self.mtsc_curvature_check = self.map_turn_speed_controller and self.params.get_bool("MTSCCurvatureCheck")
+    self.params_memory.put_float("MapTargetLatA", 2 * (self.params.get_int("MTSCAggressiveness") / 100))
+
+    self.speed_limit_controller = self.CP.openpilotLongitudinalControl and self.params.get_bool("SpeedLimitController")
+    self.speed_limit_confirmation = self.speed_limit_controller and self.params.get_bool("SLCConfirmation")
+    self.speed_limit_controller_override = self.speed_limit_controller and self.params.get_int("SLCOverride")
+
+    self.vision_turn_controller = self.CP.openpilotLongitudinalControl and self.params.get_bool("VisionTurnControl")
+    self.curve_sensitivity = self.params.get_int("CurveSensitivity") / 100 if self.vision_turn_controller else 1
+    self.turn_aggressiveness = self.params.get_int("TurnAggressiveness") / 100 if self.vision_turn_controller else 1

selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py

@@ -0,0 +1,195 @@
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.numpy_fast import interp
+from openpilot.common.params import Params
+
+from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CITY_SPEED_LIMIT, CRUISING_SPEED, PROBABILITY, MovingAverageCalculator
+
+from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController
+
+SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
+SLOW_DOWN_DISTANCE = [20, 30., 50., 70., 80., 90., 105., 120.]
+TRAJECTORY_SIZE = 33
+
+class ConditionalExperimentalMode:
+  def __init__(self):
+    self.params = Params()
+    self.params_memory = Params("/dev/shm/params")
+
+    self.curve_detected = False
+    self.experimental_mode = False
+    self.lead_detected = False
+    self.red_light_detected = False
+    self.slower_lead_detected = False
+
+    self.previous_status_value = 0
+    self.previous_v_ego = 0
+    self.previous_v_lead = 0
+    self.status_value = 0
+
+    self.curvature_mac = MovingAverageCalculator()
+    self.lead_detection_mac = MovingAverageCalculator()
+    self.lead_slowing_down_mac = MovingAverageCalculator()
+    self.slow_lead_mac = MovingAverageCalculator()
+    self.slowing_down_mac = MovingAverageCalculator()
+    self.stop_light_mac = MovingAverageCalculator()
+
+    self.update_frogpilot_params()
+
+  def update(self, carState, enabled, frogpilotNavigation, lead, modelData, road_curvature, t_follow, v_ego):
+    lead_distance = lead.dRel
+    standstill = carState.standstill
+    v_lead = lead.vLead
+
+    if self.experimental_mode_via_press and enabled:
+      overridden = self.params_memory.get_int("CEStatus")
+    else:
+      overridden = 0
+
+    self.update_conditions(lead_distance, lead.status, modelData, road_curvature, standstill, t_follow, v_ego, v_lead)
+
+    condition_met = self.check_conditions(carState, frogpilotNavigation, lead, modelData, standstill, v_ego) and enabled
+    if condition_met and overridden not in {1, 3, 5} or overridden in {2, 4, 6}:
+      self.experimental_mode = True
+    else:
+      self.experimental_mode = False
+      self.status_value = 0
+
+    self.status_value = overridden if overridden in {1, 2, 3, 4, 5, 6} else self.status_value
+    if self.status_value != self.previous_status_value:
+      self.params_memory.put_int("CEStatus", self.status_value)
+      self.previous_status_value = self.status_value
+
+    if self.params_memory.get_bool("FrogPilotTogglesUpdated"):
+      self.update_frogpilot_params()
+
+  def check_conditions(self, carState, frogpilotNavigation, lead, modelData, standstill, v_ego):
+    if standstill:
+      self.status_value = 0
+      return self.experimental_mode
+
+    # Keep Experimental Mode active if stopping for a red light
+    if self.status_value == 15 and self.slowing_down(v_ego):
+      return True
+
+    if self.navigation and modelData.navEnabled and (frogpilotNavigation.approachingIntersection or frogpilotNavigation.approachingTurn) and (self.navigation_lead or not self.lead_detected):
+      self.status_value = 7 if frogpilotNavigation.approachingIntersection else 8
+      return True
+
+    if SpeedLimitController.experimental_mode:
+      self.status_value = 9
+      return True
+
+    if (not self.lead_detected and v_ego <= self.limit) or (self.lead_detected and v_ego <= self.limit_lead):
+      self.status_value = 10 if self.lead_detected else 11
+      return True
+
+    if self.slower_lead and self.slower_lead_detected:
+      self.status_value = 12
+      return True
+
+    if self.signal and v_ego <= CITY_SPEED_LIMIT and (carState.leftBlinker or carState.rightBlinker):
+      self.status_value = 13
+      return True
+
+    if self.curves and self.curve_detected:
+      self.status_value = 14
+      return True
+
+    if self.stop_lights and self.red_light_detected:
+      self.status_value = 15
+      return True
+
+    return False
+
+  def update_conditions(self, lead_distance, lead_status, modelData, road_curvature, standstill, t_follow, v_ego, v_lead):
+    self.lead_detection(lead_status)
+    self.road_curvature(road_curvature)
+    self.slow_lead(lead_distance, t_follow, v_ego)
+    self.stop_sign_and_light(lead_distance, modelData, standstill, v_ego, v_lead)
+
+  def lead_detection(self, lead_status):
+    self.lead_detection_mac.add_data(lead_status)
+    self.lead_detected = self.lead_detection_mac.get_moving_average() >= PROBABILITY
+
+  def lead_slowing_down(self, lead_distance, v_ego, v_lead):
+    if self.lead_detected:
+      lead_close = lead_distance < CITY_SPEED_LIMIT
+      lead_far = lead_distance >= CITY_SPEED_LIMIT and (v_lead >= self.previous_v_lead > 1 or v_lead > v_ego)
+      lead_slowing_down = v_lead < self.previous_v_lead
+      lead_stopped = v_lead < 1
+
+      self.previous_v_lead = v_lead
+
+      self.lead_slowing_down_mac.add_data((lead_close or lead_slowing_down or lead_stopped) and not lead_far)
+      return self.lead_slowing_down_mac.get_moving_average() >= PROBABILITY
+    else:
+      self.lead_slowing_down_mac.reset_data()
+      self.previous_v_lead = 0
+      return False
+
+  # Determine the road curvature - Credit goes to to Pfeiferj!
+  def road_curvature(self, road_curvature):
+    lead_check = self.curves_lead or not self.lead_detected
+
+    if lead_check and not self.red_light_detected:
+      # Setting a limit of 5.0 helps prevent it triggering for red lights
+      curve_detected = 5.0 >= road_curvature > 1.6
+      curve_active = 5.0 >= road_curvature > 1.1 and self.curve_detected
+
+      self.curvature_mac.add_data(curve_detected or curve_active)
+      self.curve_detected = self.curvature_mac.get_moving_average() >= PROBABILITY
+    else:
+      self.curvature_mac.reset_data()
+      self.curve_detected = False
+
+  def slow_lead(self, lead_distance, t_follow, v_ego):
+    if self.lead_detected:
+      slower_lead_ahead = lead_distance < (v_ego - 1) * t_follow
+
+      self.slow_lead_mac.add_data(slower_lead_ahead)
+      self.slower_lead_detected = self.slow_lead_mac.get_moving_average() >= PROBABILITY
+    else:
+      self.slow_lead_mac.reset_data()
+      self.slower_lead_detected = False
+
+  def slowing_down(self, v_ego):
+    slowing_down = v_ego <= self.previous_v_ego
+    speed_check = v_ego < CRUISING_SPEED
+
+    self.previous_v_ego = v_ego
+
+    self.slowing_down_mac.add_data(slowing_down and speed_check)
+    return self.slowing_down_mac.get_moving_average() >= PROBABILITY
+
+  # Stop sign/stop light detection - Credit goes to the DragonPilot team!
+  def stop_sign_and_light(self, lead_distance, modelData, standstill, v_ego, v_lead):
+    lead_check = self.stop_lights_lead or not self.lead_slowing_down(lead_distance, v_ego, v_lead) or standstill
+
+    model_check = len(modelData.orientation.x) == len(modelData.position.x) == TRAJECTORY_SIZE
+    model_stopping = modelData.position.x[TRAJECTORY_SIZE - 1] < interp(v_ego * CV.MS_TO_KPH, SLOW_DOWN_BP, SLOW_DOWN_DISTANCE)
+
+    model_filtered = not (self.curve_detected or self.slower_lead_detected)
+
+    self.stop_light_mac.add_data(lead_check and model_check and model_stopping and model_filtered)
+    self.red_light_detected = self.stop_light_mac.get_moving_average() >= PROBABILITY
+
+  def update_frogpilot_params(self):
+    is_metric = self.params.get_bool("IsMetric")
+
+    self.curves = self.params.get_bool("CECurves")
+    self.curves_lead = self.curves and self.params.get_bool("CECurvesLead")
+
+    self.experimental_mode_via_press = self.params.get_bool("ExperimentalModeActivation")
+
+    self.limit = self.params.get_int("CESpeed") * (CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS)
+    self.limit_lead = self.params.get_int("CESpeedLead") * (CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS)
+
+    self.navigation = self.params.get_bool("CENavigation")
+    self.navigation_lead = self.navigation and self.params.get_bool("CENavigationLead")
+
+    self.signal = self.params.get_bool("CESignal")
+
+    self.slower_lead = self.params.get_bool("CESlowerLead")
+
+    self.stop_lights = self.params.get_bool("CEStopLights")
+    self.stop_lights_lead = self.stop_lights and self.params.get_bool("CEStopLightsLead")

selfdrive/frogpilot/controls/lib/frogpilot_functions.py

@@ -0,0 +1,163 @@
+import datetime
+import filecmp
+import glob
+import numpy as np
+import os
+import shutil
+import subprocess
+import time
+
+from openpilot.common.basedir import BASEDIR
+from openpilot.common.numpy_fast import interp
+from openpilot.common.params import Params
+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):
+  predicted_velocities = np.array(modelData.velocity.x)
+  curvature_ratios = np.abs(np.array(modelData.acceleration.y)) / (predicted_velocities**2)
+  return np.amax(curvature_ratios * (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
+  def run_cmd(cls, cmd, success_msg, fail_msg):
+    try:
+      subprocess.check_call(cmd)
+      print(success_msg)
+    except subprocess.CalledProcessError as e:
+      print(f"{fail_msg}: {e}")
+    except Exception as e:
+      print(f"Unexpected error occurred: {e}")
+
+  @classmethod
+  def backup_frogpilot(cls):
+    frogpilot_backup_directory = "/data/backups"
+    os.makedirs(frogpilot_backup_directory, exist_ok=True)
+
+    auto_backups = sorted(glob.glob(os.path.join(frogpilot_backup_directory, "*_auto")),
+                          key=os.path.getmtime, reverse=True)
+
+    for old_backup in auto_backups[4:]:
+      shutil.rmtree(old_backup)
+      print(f"Deleted oldest FrogPilot backup to maintain limit: {os.path.basename(old_backup)}")
+
+    branch = get_short_branch()
+    commit = get_commit_date()[12:-16]
+    backup_folder_name = f"{branch}_{commit}_auto"
+    backup_path = os.path.join(frogpilot_backup_directory, backup_folder_name)
+
+    if not os.path.exists(backup_path):
+      cmd = ['sudo', 'cp', '-a', f"{BASEDIR}", f"{backup_path}/"]
+      cls.run_cmd(cmd, f"Successfully backed up FrogPilot to {backup_folder_name}.", f"Failed to backup FrogPilot to {backup_folder_name}.")
+
+  @classmethod
+  def backup_toggles(cls):
+    params = Params()
+    params_storage = Params("/persist/params")
+
+    all_keys = params.all_keys()
+    for key in all_keys:
+      value = params.get(key)
+      if value is not None:
+        params_storage.put(key, value)
+
+    toggle_backup_directory = "/data/toggle_backups"
+    os.makedirs(toggle_backup_directory, exist_ok=True)
+
+    auto_backups = sorted(glob.glob(os.path.join(toggle_backup_directory, "*_auto")),
+                          key=os.path.getmtime, reverse=True)
+
+    for old_backup in auto_backups[9:]:
+      shutil.rmtree(old_backup)
+      print(f"Deleted oldest toggle backup to maintain limit: {os.path.basename(old_backup)}")
+
+    current_datetime = datetime.datetime.now().strftime("%Y-%m-%d_%I-%M%p").lower()
+    backup_folder_name = f"{current_datetime}_auto"
+    backup_path = os.path.join(toggle_backup_directory, backup_folder_name)
+
+    if not os.path.exists(backup_path):
+      cmd = ['sudo', 'cp', '-a', '/data/params/.', f"{backup_path}/"]
+      cls.run_cmd(cmd, f"Successfully backed up toggles to {backup_folder_name}.", f"Failed to backup toggles to {backup_folder_name}.")
+
+  @classmethod
+  def delete_logs(cls):
+    directories_to_check = []
+    for root, dirs, files in os.walk('/data/media/0/realdata/', topdown=False):
+      for name in files:
+        filepath = os.path.join(root, name)
+        if time.time() - max(os.path.getctime(filepath), os.path.getmtime(filepath)) < 3600:
+          os.remove(filepath)
+          if root not in directories_to_check:
+            directories_to_check.append(root)
+    for directory in directories_to_check:
+      try:
+        os.rmdir(directory)
+      except OSError:
+        pass
+
+  @classmethod
+  def setup_frogpilot(cls):
+    remount_cmd = ['sudo', 'mount', '-o', 'remount,rw', '/persist']
+    cls.run_cmd(remount_cmd, "Successfully remounted /persist as read-write.", "Failed to remount /persist.")
+
+    if os.path.isdir('/persist/comma/params') and os.path.isdir('/persist/params'):
+      if os.listdir('/persist/comma/params') and os.listdir('/persist/params'):
+        shutil.rmtree('/persist/comma/params')
+
+    frogpilot_boot_logo = f'{BASEDIR}/selfdrive/frogpilot/assets/other_images/frogpilot_boot_logo.png'
+    boot_logo_location = '/usr/comma/bg.jpg'
+
+    remount_cmd = ['sudo', 'mount', '-o', 'remount,rw', '/']
+    cls.run_cmd(remount_cmd, "File system remounted as read-write.", "Failed to remount file system")
+
+    if not filecmp.cmp(frogpilot_boot_logo, boot_logo_location, shallow=False):
+      copy_cmd = ['sudo', 'cp', frogpilot_boot_logo, boot_logo_location]
+      cls.run_cmd(copy_cmd, "Successfully replaced bg.jpg with frogpilot_boot_logo.png.", "Failed to replace boot logo")
+
+  @classmethod
+  def uninstall_frogpilot(cls):
+    original_boot_logo = f'{BASEDIR}/selfdrive/frogpilot/assets/other_images/bg.jpg'
+    boot_logo_location = '/usr/comma/bg.jpg'
+
+    copy_cmd = ['sudo', 'cp', original_boot_logo, boot_logo_location]
+    cls.run_cmd(copy_cmd, "Successfully restored the original boot logo.", "Failed to restore the original boot logo.")
+
+    HARDWARE.uninstall()

selfdrive/frogpilot/controls/lib/lock_doors.py

@@ -0,0 +1,39 @@
+#!/usr/bin/env python3
+import subprocess
+import sys
+import time
+
+from panda import Panda
+from openpilot.system.hardware import HARDWARE
+
+unlockCommand = [0x40, 0x05, 0x30, 0x11, 0x00, 0x40, 0x00, 0x00]
+lockCommand = [0x40, 0x05, 0x30, 0x11, 0x00, 0x80, 0x00, 0x00]
+p = Panda()
+
+def main():
+  subprocess.run(['pkill', '-f', 'openpilot'], check=True)
+
+  p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+
+  # args
+  if len(sys.argv) != 2:
+    sys.exit('usage:\n\nroot@localhost:/data/openpilot$ pkill -f openpilot\n\nroot@localhost:/data/openpilot$ doors.py --lock\n\nroot@localhost:/data/openpilot$ doors.py --unlock\n\nroot@localhost:/data/openpilot$ reboot')
+
+  if sys.argv[1]  == '--lock' or sys.argv[1]  == '-l':
+    p.can_send(0x750, bytes(unlockCommand), 0)
+    time.sleep(0.2)
+    p.can_send(0x750, bytes(lockCommand), 0)
+
+  if sys.argv[1] == '--unlock' or sys.argv[1] == '-u':
+    p.can_send(0x750, bytes(lockCommand), 0)
+    time.sleep(0.2)
+    p.can_send(0x750, bytes(unlockCommand), 0)
+
+  time.sleep(0.2)
+  p.set_safety_mode(Panda.SAFETY_TOYOTA)
+  p.send_heartbeat()
+  print('\n\n\nrelay ON again\nkthxbay\n')
+
+  HARDWARE.soft_reboot()
+
+main()

selfdrive/frogpilot/controls/lib/map_turn_speed_controller.py

@@ -0,0 +1,153 @@
+# PFEIFER - MTSC
+import json
+import math
+
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.numpy_fast import interp
+from openpilot.common.params import Params
+
+params_memory = Params("/dev/shm/params")
+
+R = 6373000.0 # approximate radius of earth in meters
+TO_RADIANS = math.pi / 180
+TO_DEGREES = 180 / math.pi
+TARGET_JERK = -0.6   # m/s^3 should match up with the long planner
+TARGET_ACCEL = -1.2  # m/s^2 should match up with the long planner
+TARGET_OFFSET = 1.0  # seconds - This controls how soon before the curve you reach the target velocity. It also helps
+                     # reach the target velocity when innacuracies in the distance modeling logic would cause overshoot.
+                     # The value is multiplied against the target velocity to determine the additional distance. This is
+                     # done to keep the distance calculations consistent but results in the offset actually being less
+                     # time than specified depending on how much of a speed diffrential there is between v_ego and the
+                     # target velocity.
+
+def calculate_accel(t, target_jerk, a_ego):
+  return a_ego  + target_jerk * t
+
+def calculate_velocity(t, target_jerk, a_ego, v_ego):
+  return v_ego + a_ego * t + target_jerk/2 * (t ** 2)
+
+def calculate_distance(t, target_jerk, a_ego, v_ego):
+  return t * v_ego + a_ego/2 * (t ** 2) + target_jerk/6 * (t ** 3)
+
+
+# points should be in radians
+# output is meters
+def distance_to_point(ax, ay, bx, by):
+  a = math.sin((bx-ax)/2)*math.sin((bx-ax)/2) + math.cos(ax) * math.cos(bx)*math.sin((by-ay)/2)*math.sin((by-ay)/2)
+  c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
+
+  return R * c  # in meters
+
+class MapTurnSpeedController:
+  def __init__(self):
+    self.target_lat = 0.0
+    self.target_lon = 0.0
+    self.target_v = 0.0
+
+  def target_speed(self, v_ego, a_ego) -> float:
+    lat = 0.0
+    lon = 0.0
+    try:
+      position = json.loads(params_memory.get("LastGPSPosition"))
+      lat = position["latitude"]
+      lon = position["longitude"]
+    except: return 0.0
+
+    try:
+      target_velocities = json.loads(params_memory.get("MapTargetVelocities"))
+    except: return 0.0
+
+    min_dist = 1000
+    min_idx = 0
+    distances = []
+
+    # find our location in the path
+    for i in range(len(target_velocities)):
+      target_velocity = target_velocities[i]
+      tlat = target_velocity["latitude"]
+      tlon = target_velocity["longitude"]
+      d = distance_to_point(lat * TO_RADIANS, lon * TO_RADIANS, tlat * TO_RADIANS, tlon * TO_RADIANS)
+      distances.append(d)
+      if d < min_dist:
+        min_dist = d
+        min_idx = i
+
+    # only look at values from our current position forward
+    forward_points = target_velocities[min_idx:]
+    forward_distances = distances[min_idx:]
+
+    # find velocities that we are within the distance we need to adjust for
+    valid_velocities = []
+    for i in range(len(forward_points)):
+      target_velocity = forward_points[i]
+      tlat = target_velocity["latitude"]
+      tlon = target_velocity["longitude"]
+      tv = target_velocity["velocity"]
+      if tv > v_ego:
+        continue
+
+      d = forward_distances[i]
+
+      a_diff = (a_ego - TARGET_ACCEL)
+      accel_t = abs(a_diff / TARGET_JERK)
+      min_accel_v = calculate_velocity(accel_t, TARGET_JERK, a_ego, v_ego)
+
+      max_d = 0
+      if tv > min_accel_v:
+        # calculate time needed based on target jerk
+        a = 0.5 * TARGET_JERK
+        b = a_ego
+        c = v_ego - tv
+        t_a = -1 * ((b**2 - 4 * a * c) ** 0.5 + b) / 2 * a
+        t_b = ((b**2 - 4 * a * c) ** 0.5 - b) / 2 * a
+        if not isinstance(t_a, complex) and t_a > 0:
+          t = t_a
+        else:
+          t = t_b
+        if isinstance(t, complex):
+          continue
+
+        max_d = max_d + calculate_distance(t, TARGET_JERK, a_ego, v_ego)
+
+      else:
+        t = accel_t
+        max_d = calculate_distance(t, TARGET_JERK, a_ego, v_ego)
+
+        # calculate additional time needed based on target accel
+        t = abs((min_accel_v - tv) / TARGET_ACCEL)
+        max_d += calculate_distance(t, 0, TARGET_ACCEL, min_accel_v)
+
+      if d < max_d + tv * TARGET_OFFSET:
+        valid_velocities.append((float(tv), tlat, tlon))
+
+    # Find the smallest velocity we need to adjust for
+    min_v = 100.0
+    target_lat = 0.0
+    target_lon = 0.0
+    for tv, lat, lon in valid_velocities:
+      if tv < min_v:
+        min_v = tv
+        target_lat = lat
+        target_lon = lon
+
+    if self.target_v < min_v and not (self.target_lat == 0 and self.target_lon == 0):
+      for i in range(len(forward_points)):
+        target_velocity = forward_points[i]
+        tlat = target_velocity["latitude"]
+        tlon = target_velocity["longitude"]
+        tv = target_velocity["velocity"]
+        if tv > v_ego:
+          continue
+
+        if tlat == self.target_lat and tlon == self.target_lon and tv == self.target_v:
+          return float(self.target_v)
+      # not found so lets reset
+      self.target_v = 0.0
+      self.target_lat = 0.0
+      self.target_lon = 0.0
+
+    self.target_v = min_v
+    self.target_lat = target_lat
+    self.target_lon = target_lon
+
+    return min_v

selfdrive/frogpilot/controls/lib/model_manager.py

@@ -0,0 +1,102 @@
+import os
+import stat
+import time
+import urllib.request
+
+from openpilot.common.params import Params
+from openpilot.system.version import get_short_branch
+
+VERSION = 'v1' if get_short_branch() == "FrogPilot" else 'v2'
+REPOSITORY_URL = 'https://github.com/FrogAi/FrogPilot-Resources/releases/download'
+
+DEFAULT_MODEL = "wd-40"
+DEFAULT_MODEL_NAME = "WD40 (Default)"
+MODELS_PATH = '/data/models'
+
+NAVIGATIONLESS_MODELS = {"radical-turtle", "wd-40"}
+RADARLESS_MODELS = {"radical-turtle"}
+
+params = Params()
+params_memory = Params("/dev/shm/params")
+
+def delete_deprecated_models():
+  populate_models()
+
+  available_models = params.get("AvailableModels", encoding='utf-8').split(',')
+
+  if available_models:
+    current_model = params.get("Model", encoding='utf-8')
+    current_model_file = os.path.join(MODELS_PATH, f"{current_model}.thneed")
+
+    if current_model not in available_models or not os.path.exists(current_model_file):
+      params.put("Model", DEFAULT_MODEL)
+      params.put("ModelName", DEFAULT_MODEL_NAME)
+
+    for model_file in os.listdir(MODELS_PATH):
+      if model_file.endswith('.thneed') and model_file[:-7] not in available_models:
+        os.remove(os.path.join(MODELS_PATH, model_file))
+  else:
+    params.put("Model", DEFAULT_MODEL)
+    params.put("ModelName", DEFAULT_MODEL_NAME)
+
+def download_model():
+  model = params_memory.get("ModelToDownload", encoding='utf-8')
+  model_path = os.path.join(MODELS_PATH, f"{model}.thneed")
+  url = f"{REPOSITORY_URL}/{model}/{model}.thneed"
+
+  os.makedirs(MODELS_PATH, exist_ok=True)
+
+  for attempt in range(5):
+    try:
+      with urllib.request.urlopen(url) as f:
+        total_file_size = int(f.getheader('Content-Length'))
+        if total_file_size == 0:
+          raise ValueError("File is empty")
+
+        with open(model_path, 'wb') as output:
+          current_file_size = 0
+          while chunk := f.read(8192):
+            output.write(chunk)
+            current_file_size += len(chunk)
+            progress = (current_file_size / total_file_size) * 100
+            params_memory.put_int("ModelDownloadProgress", int(progress))
+          os.fsync(output)
+
+      if os.path.getsize(model_path) == total_file_size:
+        print(f"Successfully downloaded the {model} model!")
+        break
+      else:
+        raise Exception("Downloaded model file size does not match expected size. Retrying...")
+
+    except Exception as e:
+      print(f"Attempt {attempt + 1} failed with error: {e}. Retrying...")
+      if os.path.exists(model_path):
+        os.remove(model_path)
+      time.sleep(5)
+  else:
+    print(f"Failed to download the {model} model after {attempt + 1} attempts. Giving up... :(")
+
+def populate_models():
+  model_names_url = f"https://raw.githubusercontent.com/FrogAi/FrogPilot-Resources/master/model_names_{VERSION}.txt"
+
+  for attempt in range(5):
+    try:
+      with urllib.request.urlopen(model_names_url) as response:
+        model_info = [line.decode('utf-8').strip().split(' - ') for line in response.readlines() if ' - ' in line.decode('utf-8')]
+
+      available_models = ','.join(model[0] for model in model_info)
+      available_models_names = [model[1] for model in model_info]
+
+      params.put("AvailableModels", available_models)
+      params.put("AvailableModelsNames", ','.join(available_models_names))
+
+      current_model_name = params.get("ModelName", encoding='utf-8')
+      if current_model_name not in available_models_names and "(Default)" in current_model_name:
+        updated_model_name = current_model_name.replace("(Default)", "").strip()
+        params.put("ModelName", updated_model_name)
+
+    except Exception as e:
+      print(f"Failed to update models list. Error: {e}. Retrying...")
+      time.sleep(5)
+  else:
+    print(f"Failed to update models list after 5 attempts. Giving up... :(")

selfdrive/frogpilot/controls/lib/speed_limit_controller.py

@@ -0,0 +1,160 @@
+# PFEIFER - SLC - Modified by FrogAi for FrogPilot
+import json
+import math
+
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.params import Params
+
+R = 6373000.0  # approximate radius of earth in meters
+TO_RADIANS = math.pi / 180
+TO_DEGREES = 180 / math.pi
+
+# points should be in radians
+# output is meters
+def distance_to_point(ax, ay, bx, by):
+  a = math.sin((bx - ax) / 2) ** 2 + math.cos(ax) * math.cos(bx) * math.sin((by - ay) / 2) ** 2
+  c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
+  return R * c  # in meters
+
+class SpeedLimitController:
+  def __init__(self):
+    self.params = Params()
+    self.params_memory = Params("/dev/shm/params")
+
+    self.car_speed_limit = 0  # m/s
+    self.map_speed_limit = 0  # m/s
+    self.nav_speed_limit = 0  # m/s
+    self.prv_speed_limit = 0  # m/s
+
+    self.lat = 0  # deg
+    self.lon = 0  # deg
+
+    self.update_frogpilot_params()
+
+  def update(self, v_ego):
+    self.car_speed_limit = self.get_param_memory("CarSpeedLimit")
+    self.write_map_state(v_ego)
+    self.nav_speed_limit = self.get_param_memory("NavSpeedLimit")
+
+    if self.params_memory.get_bool("FrogPilotTogglesUpdated"):
+      self.update_frogpilot_params()
+
+  def get_param_memory(self, key, is_json=False, default=None):
+    try:
+      data = self.params_memory.get(key)
+      if not is_json and data is not None:
+        return float(data.decode('utf-8'))
+      return json.loads(data) if is_json else data
+    except:
+      return default
+
+  def write_map_state(self, v_ego):
+    self.map_speed_limit = self.get_param_memory("MapSpeedLimit")
+
+    next_map_speed_limit = self.get_param_memory("NextMapSpeedLimit", is_json=True, default={})
+    next_map_speed_limit_value = next_map_speed_limit.get("speedlimit", 0)
+    next_map_speed_limit_lat = next_map_speed_limit.get("latitude", 0)
+    next_map_speed_limit_lon = next_map_speed_limit.get("longitude", 0)
+
+    position = self.get_param_memory("LastGPSPosition", is_json=True, default={})
+    self.lat = position.get("latitude", 0)
+    self.lon = position.get("longitude", 0)
+
+    if self.prv_speed_limit < next_map_speed_limit_value > 1:
+      d = distance_to_point(self.lat * TO_RADIANS, self.lon * TO_RADIANS,
+                            next_map_speed_limit_lat * TO_RADIANS, next_map_speed_limit_lon * TO_RADIANS)
+      max_d = self.map_speed_lookahead_higher * v_ego
+      if d < max_d:
+        self.map_speed_limit = next_map_speed_limit_value
+
+    if self.prv_speed_limit > next_map_speed_limit_value > 1:
+      d = distance_to_point(self.lat * TO_RADIANS, self.lon * TO_RADIANS,
+                            next_map_speed_limit_lat * TO_RADIANS, next_map_speed_limit_lon * TO_RADIANS)
+      max_d = self.map_speed_lookahead_higher_lower * v_ego
+      if d < max_d:
+        self.map_speed_limit = next_map_speed_limit_value
+
+  @property
+  def speed_limit(self):
+    limits = [self.car_speed_limit, self.map_speed_limit, self.nav_speed_limit]
+    filtered_limits = [limit for limit in limits if limit is not None and limit > 1]
+
+    if self.highest and filtered_limits:
+      return float(max(filtered_limits))
+    elif self.lowest and filtered_limits:
+      return float(min(filtered_limits))
+
+    speed_limits = {
+      "Dashboard": self.car_speed_limit,
+      "Offline Maps": self.map_speed_limit,
+      "Navigation": self.nav_speed_limit,
+    }
+
+    priorities = [
+      self.speed_limit_priority1,
+      self.speed_limit_priority2,
+      self.speed_limit_priority3,
+    ]
+
+    for priority in priorities:
+      if priority in speed_limits and speed_limits[priority] in filtered_limits:
+        return float(speed_limits[priority])
+
+    if self.use_previous_limit:
+      return float(self.prv_speed_limit)
+
+    return 0
+
+  @property
+  def offset(self):
+    if self.speed_limit < 13.5:
+      return self.offset1
+    elif self.speed_limit < 24:
+      return self.offset2
+    elif self.speed_limit < 29:
+      return self.offset3
+    else:
+      return self.offset4
+
+  @property
+  def desired_speed_limit(self):
+    if self.speed_limit > 1:
+      self.update_previous_limit(self.speed_limit)
+      return self.speed_limit + self.offset
+    else:
+      return 0
+
+  def update_previous_limit(self, speed_limit):
+    if self.prv_speed_limit != speed_limit:
+      self.params.put_float("PreviousSpeedLimit", speed_limit)
+      self.prv_speed_limit = speed_limit
+
+  @property
+  def experimental_mode(self):
+    return self.speed_limit == 0 and self.use_experimental_mode
+
+  def update_frogpilot_params(self):
+    conversion = CV.KPH_TO_MS if self.params.get_bool("IsMetric") else CV.MPH_TO_MS
+
+    self.map_speed_lookahead_higher = self.params.get_int("SLCLookaheadHigher")
+    self.map_speed_lookahead_lower = self.params.get_int("SLCLookaheadLower")
+
+    self.offset1 = self.params.get_int("Offset1") * conversion
+    self.offset2 = self.params.get_int("Offset2") * conversion
+    self.offset3 = self.params.get_int("Offset3") * conversion
+    self.offset4 = self.params.get_int("Offset4") * conversion
+
+    self.speed_limit_priority1 = self.params.get("SLCPriority1", encoding='utf-8')
+    self.speed_limit_priority2 = self.params.get("SLCPriority2", encoding='utf-8')
+    self.speed_limit_priority3 = self.params.get("SLCPriority3", encoding='utf-8')
+
+    self.highest = self.speed_limit_priority1 == "Highest"
+    self.lowest = self.speed_limit_priority1 == "Lowest"
+
+    slc_fallback = self.params.get_int("SLCFallback")
+    self.use_experimental_mode = slc_fallback == 1
+    self.use_previous_limit = slc_fallback == 2
+
+    self.prv_speed_limit = self.params.get_float("PreviousSpeedLimit")
+
+SpeedLimitController = SpeedLimitController()

selfdrive/frogpilot/controls/lib/theme_manager.py

@@ -0,0 +1,89 @@
+import datetime
+import time
+import threading
+
+from openpilot.common.params import Params
+
+class ThemeManager:
+  def __init__(self):
+    self.params = Params()
+    self.params_memory = Params("/dev/shm/params")
+
+    self.previous_theme_id = 0
+
+  @staticmethod
+  def calculate_easter(year):
+    a = year % 19
+    b = year // 100
+    c = year % 100
+    d = b // 4
+    e = b % 4
+    f = (b + 8) // 25
+    g = (b - f + 1) // 3
+    h = (19 * a + b - d - g + 15) % 30
+    i = c // 4
+    k = c % 4
+    l = (32 + 2 * e + 2 * i - h - k) % 7
+    m = (a + 11 * h + 22 * l) // 451
+    month = (h + l - 7 * m + 114) // 31
+    day = ((h + l - 7 * m + 114) % 31) + 1
+    return datetime.datetime(year, month, day)
+
+  @staticmethod
+  def calculate_thanksgiving(year):
+    fourth_thursday = 4
+    november_first = datetime.datetime(year, 11, 1)
+    day_of_week = november_first.weekday()
+    thanksgiving = november_first + datetime.timedelta(days=((fourth_thursday - day_of_week) % 7) + 21)
+    return thanksgiving
+
+  @staticmethod
+  def is_within_week_of(target_date, current_date):
+    start_of_week = target_date - datetime.timedelta(days=target_date.weekday())
+    end_of_week = start_of_week + datetime.timedelta(days=6)
+    return start_of_week <= current_date <= end_of_week
+
+  def update_holiday(self):
+    current_date = datetime.datetime.now()
+    year = current_date.year
+
+    holidays = {
+      "april_fools": (datetime.datetime(year, 4, 1), 1),
+      "christmas_week": (datetime.datetime(year, 12, 25), 2),
+      "cinco_de_mayo": (datetime.datetime(year, 5, 5), 3),
+      "easter_week": (self.calculate_easter(year), 4),
+      "fourth_of_july": (datetime.datetime(year, 7, 4), 5),
+      "halloween_week": (datetime.datetime(year, 10, 31), 6),
+      "new_years": (datetime.datetime(year, 1, 1), 7),
+      "st_patricks": (datetime.datetime(year, 3, 17), 8),
+      "thanksgiving_week": (self.calculate_thanksgiving(year), 9),
+      "valentines": (datetime.datetime(year, 2, 14), 10),
+      "world_frog_day": (datetime.datetime(year, 3, 20), 11)
+    }
+
+    for holiday, (date, theme_id) in holidays.items():
+      if holiday.endswith("_week"):
+        if self.is_within_week_of(date, current_date):
+          if theme_id != self.previous_theme_id:
+            update_holiday = threading.Thread(target=update_holiday_theme, args=(theme_id,))
+            update_holiday.start()
+          self.previous_theme_id = theme_id
+          return
+
+      elif current_date.date() == date.date():
+        if theme_id != self.previous_theme_id:
+          update_holiday = threading.Thread(target=update_holiday_theme, args=(theme_id,))
+          update_holiday.start()
+        self.previous_theme_id = theme_id
+        return
+
+    if self.previous_theme_id != 0:
+      update_holiday = threading.Thread(target=update_holiday_theme, args=(0,))
+      update_holiday.start()
+    self.previous_theme_id = 0
+
+  def update_holiday_theme(self, theme_id):
+    self.params_memory.put_int("CurrentHolidayTheme", theme_id)
+    self.params_memory.put_bool("FrogPilotTogglesUpdated", True)
+    time.sleep(1)
+    self.params_memory.put_bool("FrogPilotTogglesUpdated", False)