FrogPilot 0.9.7

2026-09-02 06:03:43 +08:00 · 2024-06-27 10:22:08 -07:00
parent da00915ac8
commit b705b02e70
682 changed files with 181798 additions and 1348 deletions
@@ -14,6 +14,7 @@ from openpilot.selfdrive.locationd.models.car_kf import CarKalman, ObservationKi
 from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR
 from openpilot.common.swaglog import cloudlog

+from openpilot.selfdrive.frogpilot.frogpilot_variables import get_frogpilot_toggles, params_memory

 MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL  # Max 20 deg/s
 ROLL_MAX_DELTA = math.radians(20.0) * DT_MDL  # 20deg in 1 second is well within curvature limits
@@ -96,8 +97,9 @@ class ParamsLearner:
      self.steering_angle = msg.steeringAngleDeg
      self.speed = msg.vEgo

+      complex_dynamics = abs(msg.aEgo) > 1.0 or abs(msg.steeringRateDeg) > 20
      in_linear_region = abs(self.steering_angle) < 45
-      self.active = self.speed > MIN_ACTIVE_SPEED and in_linear_region
+      self.active = self.speed > MIN_ACTIVE_SPEED and in_linear_region and not complex_dynamics

      if self.active:
        self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[math.radians(msg.steeringAngleDeg)]]))
@@ -124,7 +126,7 @@ def main():
  REPLAY = bool(int(os.getenv("REPLAY", "0")))

  pm = messaging.PubMaster(['liveParameters'])
-  sm = messaging.SubMaster(['liveLocationKalman', 'carState'], poll='liveLocationKalman')
+  sm = messaging.SubMaster(['liveLocationKalman', 'carState', 'frogpilotPlan'], poll='liveLocationKalman')

  params_reader = Params()
  # wait for stats about the car to come in from controls
@@ -182,6 +184,9 @@ def main():
  total_offset_valid = True
  roll_valid = True

+  # FrogPilot variables
+  frogpilot_toggles = get_frogpilot_toggles()
+
  while True:
    sm.update()
    if sm.all_checks():
@@ -191,6 +196,12 @@ def main():
          learner.handle_log(t, which, sm[which])

    if sm.updated['liveLocationKalman']:
+      location = sm['liveLocationKalman']
+      if (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid and location.gpsOK:
+        bearing = math.degrees(location.calibratedOrientationNED.value[2])
+        lat = location.positionGeodetic.value[0]
+        lon = location.positionGeodetic.value[1]
+        params_memory.put("LastGPSPosition", json.dumps({"latitude": lat, "longitude": lon, "bearing": bearing}))
      x = learner.kf.x
      P = np.sqrt(learner.kf.P.diagonal())
      if not all(map(math.isfinite, x)):
@@ -219,7 +230,7 @@ def main():
      liveParameters = msg.liveParameters
      liveParameters.posenetValid = True
      liveParameters.sensorValid = sensors_valid
-      liveParameters.steerRatio = float(x[States.STEER_RATIO].item())
+      liveParameters.steerRatio = float(x[States.STEER_RATIO].item() if not frogpilot_toggles.use_custom_steer_ratio else frogpilot_toggles.steer_ratio)
      liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item())
      liveParameters.roll = roll
      liveParameters.angleOffsetAverageDeg = angle_offset_average
@@ -232,6 +243,8 @@ def main():
        0.2 <= liveParameters.stiffnessFactor <= 5.0,
        min_sr <= liveParameters.steerRatio <= max_sr,
      ))
+      if CP.carFingerprint == "RAM_HD" or CP.carName == "subaru" and CP.lateralTuning.which() == "torque":
+        liveParameters.valid = True
      liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item())
      liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item())
      liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET].item())
@@ -255,6 +268,9 @@ def main():

      pm.send('liveParameters', msg)

+    # Update FrogPilot parameters
+    if sm['frogpilotPlan'].togglesUpdated:
+      frogpilot_toggles = get_frogpilot_toggles()

 if __name__ == "__main__":
  main()
@@ -11,6 +11,8 @@ from openpilot.common.swaglog import cloudlog
 from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
 from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator

+from openpilot.selfdrive.frogpilot.frogpilot_variables import get_frogpilot_toggles
+
 HISTORY = 5  # secs
 POINTS_PER_BUCKET = 1500
 MIN_POINTS_TOTAL = 4000
@@ -179,7 +181,7 @@ class TorqueEstimator(ParameterEstimator):
        if all(active) and (not any(steer_override)) and (vego > MIN_VEL) and (abs(steer) > STEER_MIN_THRESHOLD) and (abs(lateral_acc) <= LAT_ACC_THRESHOLD):
          self.filtered_points.add_point(float(steer), float(lateral_acc))

-  def get_msg(self, valid=True, with_points=False):
+  def get_msg(self, valid=True, with_points=False, frogpilot_toggles=None):
    msg = messaging.new_message('liveTorqueParameters')
    msg.valid = valid
    liveTorqueParameters = msg.liveTorqueParameters
@@ -207,9 +209,9 @@ class TorqueEstimator(ParameterEstimator):
    if with_points:
      liveTorqueParameters.points = self.filtered_points.get_points()[:, [0, 2]].tolist()

-    liveTorqueParameters.latAccelFactorFiltered = float(self.filtered_params['latAccelFactor'].x)
+    liveTorqueParameters.latAccelFactorFiltered = float(self.filtered_params['latAccelFactor'].x if not frogpilot_toggles.use_custom_lat_accel_factor else frogpilot_toggles.steer_lat_accel_factor)
    liveTorqueParameters.latAccelOffsetFiltered = float(self.filtered_params['latAccelOffset'].x)
-    liveTorqueParameters.frictionCoefficientFiltered = float(self.filtered_params['frictionCoefficient'].x)
+    liveTorqueParameters.frictionCoefficientFiltered = float(self.filtered_params['frictionCoefficient'].x if not frogpilot_toggles.use_custom_steer_friction else frogpilot_toggles.steer_friction)
    liveTorqueParameters.totalBucketPoints = len(self.filtered_points)
    liveTorqueParameters.decay = self.decay
    liveTorqueParameters.maxResets = self.resets
@@ -220,12 +222,18 @@ def main(demo=False):
  config_realtime_process([0, 1, 2, 3], 5)

  pm = messaging.PubMaster(['liveTorqueParameters'])
-  sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveLocationKalman'], poll='liveLocationKalman')
+  sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveLocationKalman', 'frogpilotPlan'], poll='liveLocationKalman')

  params = Params()
  with car.CarParams.from_bytes(params.get("CarParams", block=True)) as CP:
    estimator = TorqueEstimator(CP)

+  # FrogPilot variables
+  frogpilot_toggles = get_frogpilot_toggles()
+
+  if not frogpilot_toggles.liveValid:
+    estimator = TorqueEstimator(CP, True)
+
  while True:
    sm.update()
    if sm.all_checks():
@@ -236,13 +244,17 @@ def main(demo=False):

    # 4Hz driven by liveLocationKalman
    if sm.frame % 5 == 0:
-      pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks()))
+      pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks(), with_points=False, frogpilot_toggles=frogpilot_toggles))

    # Cache points every 60 seconds while onroad
    if sm.frame % 240 == 0:
-      msg = estimator.get_msg(valid=sm.all_checks(), with_points=True)
+      msg = estimator.get_msg(valid=sm.all_checks(), with_points=True, frogpilot_toggles=frogpilot_toggles)
      params.put_nonblocking("LiveTorqueParameters", msg.to_bytes())

+    # Update FrogPilot parameters
+    if sm['frogpilotPlan'].togglesUpdated:
+      frogpilot_toggles = get_frogpilot_toggles()
+
 if __name__ == "__main__":
  import argparse
  parser = argparse.ArgumentParser(description='Process the --demo argument.')