mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-14 19:44:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

New planplus

Browse Source
This commit is contained in:
firestar5683
2025-12-02 11:53:45 -06:00
parent de1e30bef6
commit f0b6df65a0
3 changed files with 13 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
frogpilot/tinygrad_modeld/fill_model_msg.py
+3 -4
View File
@@ -11,16 +11,15 @@ ConfidenceClass = log.ModelDataV2.ConfidenceClass
# Return curvature for lateral action. If the model outputs desired_curvature and we're not in mlsim mode,
# use it directly; otherwise derive from the plan using yaw and yaw-rate.
def get_curvature_from_output(output: dict, v_ego: float, lat_action_t: float, mlsim: bool) -> float:
def get_curvature_from_output(output: dict, plan: np.ndarray, v_ego: float, lat_action_t: float, mlsim: bool) -> float:
if not mlsim:
desired = output.get('desired_curvature')
if desired is not None:
return float(desired[0, 0])
plan_out = output['plan'][0]
# Use yaw (index 2) and yaw_rate (index 2)
theta = plan_out[:, Plan.T_FROM_CURRENT_EULER][:, 2]
theta_dot = plan_out[:, Plan.ORIENTATION_RATE][:, 2]
theta = plan[:, Plan.T_FROM_CURRENT_EULER][:, 2]
theta_dot = plan[:, Plan.ORIENTATION_RATE][:, 2]
return float(get_curvature_from_plan(theta, theta_dot, ModelConstants.T_IDXS, v_ego, lat_action_t))