The rest

selfdrive/controls/controlsd.py

@@ -988,7 +988,8 @@ class Controls:
 
         # Update FrogPilot parameters
         if self.params_memory.get_bool("FrogPilotTogglesUpdated"):
-          self.update_frogpilot_params()
+          updateFrogPilotParams = threading.Thread(target=self.update_frogpilot_params)
+          updateFrogPilotParams.start()
 
     except SystemExit:
       e.set()
@@ -1010,7 +1011,7 @@ class Controls:
     self.green_light_alert = self.params.get_bool("GreenLightAlert")
 
     lateral_tune = self.params.get_bool("LateralTune")
-    self.steer_ratio = self.params.get_int("SteerRatio") / 100 if lateral_tune else self.params.get_int("SteerRatioStock") / 100
+    self.steer_ratio = self.params.get_float("SteerRatio") if lateral_tune else self.params.get_float("SteerRatioStock")
 
     longitudinal_tune = self.params.get_bool("LongitudinalTune")
     self.sport_plus = self.params.get_int("AccelerationProfile") == 3 and longitudinal_tune

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -56,7 +56,7 @@ T_DIFFS = np.diff(T_IDXS, prepend=[0.])
 COMFORT_BRAKE = 2.5
 STOP_DISTANCE = 6.0
 
-def get_jerk_factor(custom_personalities, aggressive_jerk, standard_jerk, relaxed_jerk, personality=log.LongitudinalPersonality.standard):
+def get_jerk_factor(custom_personalities=False, aggressive_jerk=0.5, standard_jerk=1.0, relaxed_jerk=1.0, personality=log.LongitudinalPersonality.standard):
   if custom_personalities:
     if personality==log.LongitudinalPersonality.relaxed:
       return relaxed_jerk
@@ -241,6 +241,13 @@ def gen_long_ocp():
 
 class LongitudinalMpc:
   def __init__(self, mode='acc'):
+    # FrogPilot variables
+    self.safe_obstacle_distance = 0
+    self.safe_obstacle_distance_stock = 0
+    self.stopped_equivalence_factor = 0
+    self.t_follow = 0
+    self.t_follow_offset = 1
+
     self.mode = mode
     self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
     self.reset()
@@ -292,8 +299,9 @@ class LongitudinalMpc:
     for i in range(N):
       self.solver.cost_set(i, 'Zl', Zl)
 
-  def set_weights(self, custom_personalities, aggressive_jerk, standard_jerk, relaxed_jerk, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard):
+  def set_weights(self, prev_accel_constraint=True, custom_personalities=False, aggressive_jerk=0.5, standard_jerk=1.0, relaxed_jerk=1.0, personality=log.LongitudinalPersonality.standard):
     jerk_factor = get_jerk_factor(custom_personalities, aggressive_jerk, standard_jerk, relaxed_jerk, personality)
+    jerk_factor /= np.mean(self.t_follow_offset)
     if self.mode == 'acc':
       a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
       cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST]
@@ -351,7 +359,7 @@ class LongitudinalMpc:
     self.cruise_min_a = min_a
     self.max_a = max_a
 
-  def update(self, radarstate, v_cruise, x, v, a, j, aggressive_acceleration, smoother_braking, custom_personalities, aggressive_follow, standard_follow, relaxed_follow, increased_stopping_distance, personality=log.LongitudinalPersonality.standard):
+  def update(self, radarstate, v_cruise, x, v, a, j, aggressive_acceleration, increased_stopping_distance, smoother_braking, custom_personalities, aggressive_follow, standard_follow, relaxed_follow, personality=log.LongitudinalPersonality.standard):
     t_follow = get_T_FOLLOW(custom_personalities, aggressive_follow, standard_follow, relaxed_follow, personality)
     self.t_follow = t_follow
     v_ego = self.x0[1]
@@ -363,8 +371,9 @@ class LongitudinalMpc:
     # Offset by FrogAi for FrogPilot for a more natural takeoff with a lead
     if aggressive_acceleration:
       distance_factor = np.maximum(1, lead_xv_0[:,0] - (lead_xv_0[:,1] * t_follow))
-      t_follow_offset = np.clip((lead_xv_0[:,1] - v_ego) + (STOP_DISTANCE + increased_stopping_distance - v_ego), 1, distance_factor)
-      t_follow = t_follow / t_follow_offset
+      standstill_offset = max(STOP_DISTANCE + increased_stopping_distance - (v_ego**COMFORT_BRAKE), 0)
+      self.t_follow_offset = np.clip((lead_xv_0[:,1] - v_ego) + standstill_offset, 1, distance_factor)
+      t_follow = t_follow / self.t_follow_offset
 
     # Offset by FrogAi for FrogPilot for a more natural approach to a slower lead
     if smoother_braking:

selfdrive/controls/lib/longitudinal_planner.py

@@ -135,7 +135,7 @@ class LongitudinalPlanner:
     accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
     accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
 
-    self.mpc.set_weights(frogpilot_planner.custom_personalities, frogpilot_planner.aggressive_jerk, frogpilot_planner.standard_jerk, frogpilot_planner.relaxed_jerk, prev_accel_constraint, personality=self.personality)
+    self.mpc.set_weights(prev_accel_constraint, frogpilot_planner.custom_personalities, frogpilot_planner.aggressive_jerk, frogpilot_planner.standard_jerk, frogpilot_planner.relaxed_jerk, personality=self.personality)
     self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
     x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)

selfdrive/controls/plannerd.py

@@ -1,6 +1,7 @@
 #!/usr/bin/env python3
 import os
 import numpy as np
+import threading
 from cereal import car
 from openpilot.common.params import Params
 from openpilot.common.realtime import Priority, config_realtime_process
@@ -41,7 +42,7 @@ def plannerd_thread():
 
   debug_mode = bool(int(os.getenv("DEBUG", "0")))
 
-  frogpilot_planner = FrogPilotPlanner(params)
+  frogpilot_planner = FrogPilotPlanner(params, params_memory)
   longitudinal_planner = LongitudinalPlanner(CP)
   lateral_planner = LateralPlanner(CP, debug=debug_mode)
 
@@ -60,7 +61,8 @@ def plannerd_thread():
       publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner)
 
     if params_memory.get_bool("FrogPilotTogglesUpdated"):
-      frogpilot_planner.update_frogpilot_params(params)
+      updateFrogPilotToggles = threading.Thread(target=frogpilot_planner.update_frogpilot_params, args=(params, params_memory))
+      updateFrogPilotToggles.start()
 
 def main():
   plannerd_thread()