mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-30 19:33:45 +08:00
Desires
This commit is contained in:
firestar5683
@@ -32,13 +32,13 @@ def create_lat_ctl_msg(packer, CAN: CanBus, active: bool, ramp_type: int, precis
|
||||
|
||||
|
||||
def create_lat_ctl2_msg(packer, CAN: CanBus, mode: int, ramp_type: int, precision_type: int,
|
||||
curvature: float, curvature_rate: float, counter: int):
|
||||
curvature: float, curvature_rate: float, counter: int, path_angle: float = 0.0):
|
||||
values = {
|
||||
"LatCtl_D2_Rq": mode,
|
||||
"LatCtlRampType_D_Rq": ramp_type,
|
||||
"LatCtlPrecision_D_Rq": precision_type,
|
||||
"LatCtlPathOffst_L_Actl": 0.0,
|
||||
"LatCtlPath_An_Actl": 0.0,
|
||||
"LatCtlPath_An_Actl": path_angle,
|
||||
"LatCtlCurv_No_Actl": curvature,
|
||||
"LatCtlCrv_NoRate2_Actl": curvature_rate,
|
||||
"HandsOffCnfm_B_Rq": 0,
|
||||
|
||||
@@ -78,6 +78,13 @@ MACH_E_SHARP_DIRECTION_CHANGE_MIN_ACCEL = 1.8
|
||||
MACH_E_SHARP_DIRECTION_CHANGE_FULL_ACCEL = 2.2
|
||||
MACH_E_SHARP_DIRECTION_CHANGE_MIN_LAG_CURVATURE = -0.0005
|
||||
MACH_E_SHARP_DIRECTION_CHANGE_FULL_LAG_CURVATURE = 0.0008
|
||||
MACH_E_UNDERSTEER_ERROR_MAX = 0.006
|
||||
MACH_E_UNDERSTEER_ERROR_MIN_DEFICIT = 0.002
|
||||
MACH_E_UNDERSTEER_ERROR_FULL_DEFICIT = 0.004
|
||||
MACH_E_PATH_ANGLE_MAX = 0.16
|
||||
MACH_E_PATH_ANGLE_STEP = 0.055
|
||||
MACH_E_PATH_ANGLE_FADE_START_SPEED = 8.0
|
||||
MACH_E_PATH_ANGLE_MAX_SPEED = 8.8
|
||||
FORD_CURVATURE_LOOKAHEAD = {
|
||||
CAR.FORD_EXPLORER_MK6: 0.20,
|
||||
}
|
||||
@@ -99,6 +106,7 @@ MANUAL_TURN_RECOVERY_SECONDS = 0.25
|
||||
class FordLateralResult:
|
||||
curvature: float = 0.0
|
||||
curvature_rate: float = 0.0
|
||||
path_angle: float = 0.0
|
||||
ramp_type: int = 0
|
||||
precision_type: int = 1
|
||||
active: bool = False
|
||||
@@ -162,6 +170,7 @@ class FordLateralController:
|
||||
self.manual_turn_direction = 0.0
|
||||
self.curvature_samples = deque(maxlen=max(2, round(0.3 / STEER_DT)))
|
||||
self.curvature_last = 0.0
|
||||
self.path_angle_last = 0.0
|
||||
self.desired_curvature_last = 0.0
|
||||
self._frame = 0
|
||||
self._update_params()
|
||||
@@ -211,6 +220,37 @@ class FordLateralController:
|
||||
def _current_curvature(CS) -> float:
|
||||
return -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1)
|
||||
|
||||
def _curvature_error_limit(self, requested: float, desired: float, current: float, v_ego: float,
|
||||
steering_pressed: bool, lane_change: bool) -> float:
|
||||
base = CarControllerParams.CURVATURE_ERROR
|
||||
if (self.CP.carFingerprint != CAR.FORD_MUSTANG_MACH_E_MK1 or not self.CP.flags & FordFlags.CANFD or
|
||||
steering_pressed or lane_change or requested * desired <= 0.0 or abs(desired) < 0.003):
|
||||
return base
|
||||
deficit = np.sign(desired) * (requested - current)
|
||||
speed_weight = float(np.interp(v_ego, [9.0, 10.0, 14.0, 16.0], [0.0, 1.0, 1.0, 0.0]))
|
||||
deficit_weight = float(np.interp(
|
||||
deficit, [MACH_E_UNDERSTEER_ERROR_MIN_DEFICIT, MACH_E_UNDERSTEER_ERROR_FULL_DEFICIT], [0.0, 1.0]))
|
||||
return base + (MACH_E_UNDERSTEER_ERROR_MAX - base) * speed_weight * deficit_weight
|
||||
|
||||
def _path_angle_assist(self, requested: float, desired: float, applied: float, v_ego: float,
|
||||
steering_pressed: bool, lane_change: bool) -> float:
|
||||
target = 0.0
|
||||
if (self.CP.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and self.CP.flags & FordFlags.CANFD and
|
||||
not steering_pressed and not lane_change and 3.0 <= v_ego < MACH_E_PATH_ANGLE_MAX_SPEED and
|
||||
requested * desired > 0.0 and requested * applied > 0.0 and
|
||||
abs(requested) > 0.021 and abs(desired) > 0.021 and abs(applied) >= 0.0195):
|
||||
max_curvature = MAX_LATERAL_ACCEL / v_ego ** 2
|
||||
residual = max(0.0, min(abs(requested), abs(desired), max_curvature) - abs(applied))
|
||||
speed_weight = float(np.interp(
|
||||
v_ego, [MACH_E_PATH_ANGLE_FADE_START_SPEED, MACH_E_PATH_ANGLE_MAX_SPEED], [1.0, 0.0]))
|
||||
target = float(np.sign(applied) * min(residual * v_ego * speed_weight, MACH_E_PATH_ANGLE_MAX))
|
||||
if target == 0.0 or target * self.path_angle_last < 0.0:
|
||||
self.path_angle_last = 0.0
|
||||
else:
|
||||
self.path_angle_last = float(np.clip(
|
||||
target, self.path_angle_last - MACH_E_PATH_ANGLE_STEP, self.path_angle_last + MACH_E_PATH_ANGLE_STEP))
|
||||
return self.path_angle_last
|
||||
|
||||
def _blend_and_scale(self, desired: float, predicted: float, v_ego: float, current: float = 0.0,
|
||||
allow_opposite_preview: bool = False) -> tuple[float, int]:
|
||||
blend = float(np.interp(abs(desired), [0.0, 0.001], [self.curvature_blend_low, self.curvature_blend_high]))
|
||||
@@ -436,6 +476,7 @@ class FordLateralController:
|
||||
self.manual_turn_direction = 0.0
|
||||
self.curvature_samples.clear()
|
||||
self.curvature_last = 0.0
|
||||
self.path_angle_last = 0.0
|
||||
self.desired_curvature_last = 0.0
|
||||
return FordLateralResult()
|
||||
|
||||
@@ -443,6 +484,7 @@ class FordLateralController:
|
||||
if manual_turn or CS.out.vEgoRaw < 0.1:
|
||||
self.curvature_samples.clear()
|
||||
self.curvature_last = 0.0
|
||||
self.path_angle_last = 0.0
|
||||
self.desired_curvature_last = 0.0
|
||||
return FordLateralResult(active=not (
|
||||
manual_turn and self.CP.carFingerprint in FORD_MANUAL_TURN_LATCH_CARS))
|
||||
@@ -506,13 +548,16 @@ class FordLateralController:
|
||||
self.desired_curvature_last = desired
|
||||
|
||||
if v_ego > 9.0:
|
||||
requested = float(np.clip(requested, current - CarControllerParams.CURVATURE_ERROR,
|
||||
current + CarControllerParams.CURVATURE_ERROR))
|
||||
error_limit = self._curvature_error_limit(
|
||||
requested, desired, current, v_ego, bool(CS.out.steeringPressed), self._lane_change()[0])
|
||||
requested = float(np.clip(requested, current - error_limit, current + error_limit))
|
||||
applied = float(apply_std_steer_angle_limits(
|
||||
requested, self.curvature_last, v_ego, CS.out.steeringAngleDeg, True, FORD_CURVATURE_LIMITS))
|
||||
if self.CP.flags & FordFlags.CANFD:
|
||||
max_curvature = MAX_LATERAL_ACCEL / max(v_ego, 1.0) ** 2
|
||||
applied = float(np.clip(applied, -max_curvature, max_curvature))
|
||||
path_angle = self._path_angle_assist(
|
||||
requested, desired, applied, v_ego, bool(CS.out.steeringPressed), self._lane_change()[0])
|
||||
|
||||
self.curvature_samples.append(predicted)
|
||||
curvature_rate = 0.0
|
||||
@@ -532,6 +577,7 @@ class FordLateralController:
|
||||
return FordLateralResult(
|
||||
curvature=self.curvature_last,
|
||||
curvature_rate=curvature_rate,
|
||||
path_angle=path_angle,
|
||||
ramp_type=2,
|
||||
precision_type=precision,
|
||||
active=True,
|
||||
|
||||
@@ -92,6 +92,71 @@ def test_mach_e_unwind_lag_ramps_continuously(controller, monkeypatch):
|
||||
assert controller._unwind_preview(0.010, -0.009, 0.011, 10.0) == -0.009
|
||||
|
||||
|
||||
@pytest.mark.parametrize("speed,desired,requested,current,driver,lane_change,expected", (
|
||||
(12.0, 0.012, 0.012, 0.004, False, False, 0.006),
|
||||
(12.0, -0.012, -0.012, 0.004, False, False, 0.006),
|
||||
(12.0, 0.012, 0.012, 0.010, False, False, 0.002),
|
||||
(12.0, 0.012, 0.012, 0.014, False, False, 0.002),
|
||||
(9.5, 0.012, 0.012, 0.004, False, False, 0.004),
|
||||
(15.0, 0.012, 0.012, 0.004, False, False, 0.004),
|
||||
(16.0, 0.012, 0.012, 0.004, False, False, 0.002),
|
||||
(12.0, 0.012, 0.012, 0.004, True, False, 0.002),
|
||||
(12.0, 0.012, 0.012, 0.004, False, True, 0.002),
|
||||
(12.0, 0.012, -0.004, 0.004, False, False, 0.002),
|
||||
))
|
||||
def test_mach_e_understeer_error_scope(controller, speed, desired, requested, current,
|
||||
driver, lane_change, expected):
|
||||
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
|
||||
controller.CP.flags = FordFlags.CANFD
|
||||
assert controller._curvature_error_limit(
|
||||
requested, desired, current, speed, driver, lane_change) == pytest.approx(expected)
|
||||
|
||||
|
||||
def test_understeer_error_preserves_other_fords(controller):
|
||||
controller.CP.flags = FordFlags.CANFD
|
||||
assert controller._curvature_error_limit(0.012, 0.012, 0.004, 12.0, False, False) == 0.002
|
||||
|
||||
|
||||
def test_mach_e_path_angle_assist_at_curvature_limit(controller):
|
||||
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
|
||||
controller.CP.flags = FordFlags.CANFD
|
||||
outputs = [controller._path_angle_assist(0.04, 0.04, 0.02, 7.5, False, False) for _ in range(3)]
|
||||
assert outputs == pytest.approx([0.055, 0.110, 0.150])
|
||||
assert controller._path_angle_assist(0.018, 0.018, 0.018, 7.5, False, False) == 0.0
|
||||
assert controller._path_angle_assist(-0.04, -0.04, -0.02, 7.5, False, False) == pytest.approx(-0.055)
|
||||
assert controller._path_angle_assist(-0.04, -0.04, -0.02, 7.5, True, False) == 0.0
|
||||
|
||||
|
||||
@pytest.mark.parametrize("speed,requested,desired,applied,driver,lane_change", (
|
||||
(9.0, 0.04, 0.04, 0.02, False, False),
|
||||
(7.5, 0.020, 0.04, 0.02, False, False),
|
||||
(7.5, 0.04, 0.018, 0.02, False, False),
|
||||
(7.5, 0.04, 0.04, 0.018, False, False),
|
||||
(7.5, 0.04, 0.04, 0.02, True, False),
|
||||
(7.5, 0.04, 0.04, 0.02, False, True),
|
||||
))
|
||||
def test_mach_e_path_angle_assist_is_scoped(controller, speed, requested, desired, applied, driver, lane_change):
|
||||
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
|
||||
controller.CP.flags = FordFlags.CANFD
|
||||
assert controller._path_angle_assist(requested, desired, applied, speed, driver, lane_change) == 0.0
|
||||
|
||||
|
||||
def test_path_angle_assist_preserves_other_fords(controller):
|
||||
controller.CP.flags = FordFlags.CANFD
|
||||
assert controller._path_angle_assist(0.04, 0.04, 0.02, 7.5, False, False) == 0.0
|
||||
|
||||
|
||||
def test_mach_e_path_angle_assist_is_encoded_with_curvature(controller):
|
||||
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
|
||||
controller.CP.flags = FordFlags.CANFD
|
||||
assist = controller._path_angle_assist(0.04, 0.04, 0.02, 7.5, False, False)
|
||||
packer = CANPacker("ford_lincoln_base_pt")
|
||||
can_bus = CanBus(SimpleNamespace(flags=FordFlags.CANFD, safetyConfigs=[SimpleNamespace()]))
|
||||
_, data, _ = fordcan.create_lat_ctl2_msg(packer, can_bus, 1, 2, 1, -0.02, 0.0, 0, -assist)
|
||||
encoded_angle = (((data[3] & 0x1f) << 6) | (data[4] >> 2)) * 0.0005 - 0.5
|
||||
assert encoded_angle == pytest.approx(-assist)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("sign", (-1, 1))
|
||||
def test_mach_e_unwind_anticipates_opening_curve_before_current_request_is_met(controller, monkeypatch, sign):
|
||||
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
|
||||
|
||||
@@ -1497,8 +1497,6 @@ class StarPilotVariables:
|
||||
toggle.startup_alert_bottom = self.get_value("StartupMessageBottom", cast=str, default="")
|
||||
|
||||
if toggle.simple_mode:
|
||||
toggle.alert_volume_controller = False
|
||||
|
||||
toggle.color_scheme = "stock"
|
||||
toggle.current_holiday_theme = "stock"
|
||||
toggle.holiday_themes = False
|
||||
|
||||
@@ -203,7 +203,8 @@ class Navigationd:
|
||||
"now": now,
|
||||
}
|
||||
|
||||
def _maybe_recompute(self, route: NavigationRoute | None, destination: dict[str, object] | None, progress: RouteProgress | None, route_state: dict[str, object] | None) -> None:
|
||||
def _maybe_recompute(self, route: NavigationRoute | None, destination: dict[str, object] | None,
|
||||
progress: RouteProgress | None, route_state: dict[str, object] | None) -> None:
|
||||
if route is None or destination is None or progress is None or route_state is None:
|
||||
return
|
||||
|
||||
@@ -301,6 +302,7 @@ class Navigationd:
|
||||
|
||||
state = {
|
||||
"valid": True,
|
||||
"updatedAtMonotonic": monotonic(),
|
||||
"maneuverModifier": str(payload.get("maneuverModifier") or ""),
|
||||
"maneuverType": str(payload.get("maneuverType") or ""),
|
||||
"laneCount": len(lanes),
|
||||
|
||||
@@ -70,3 +70,20 @@ def test_run_builds_one_payload_for_both_navigation_publishers():
|
||||
instruction_payload = navigationd._publish_nav_instruction.calls[0][3]
|
||||
state_payload = navigationd._publish_nav_state.calls[0][3]
|
||||
assert instruction_payload is state_payload
|
||||
|
||||
|
||||
def test_navigation_state_timestamp_refreshes_even_when_instruction_is_unchanged():
|
||||
navigationd = Navigationd.__new__(Navigationd)
|
||||
navigationd._last_nav_state = None
|
||||
navigationd.params_memory = type("Memory", (), {})()
|
||||
navigationd.params_memory.put_nonblocking = Recorder()
|
||||
navigationd.params_memory.remove = Recorder()
|
||||
payload = {"maneuverType": "turn", "maneuverModifier": "right", "maneuverDistance": 55.0}
|
||||
|
||||
navigationd._publish_nav_state(object(), object(), True, payload)
|
||||
navigationd._publish_nav_state(object(), object(), True, payload)
|
||||
|
||||
states = [args[1] for args in navigationd.params_memory.put_nonblocking.calls]
|
||||
assert len(states) == 2
|
||||
assert all(state["valid"] and state["updatedAtMonotonic"] > 0 for state in states)
|
||||
assert states[0]["updatedAtMonotonic"] < states[1]["updatedAtMonotonic"]
|
||||
|
||||
@@ -6724,6 +6724,12 @@ def setup(app):
|
||||
if key == "RivianAngleControl":
|
||||
response["message"] = "Rivian steering mode updated. The safe channel handoff is in progress."
|
||||
updated = {}
|
||||
if key in {
|
||||
"BelowSteerSpeedVolume", "DisengageVolume", "EngageVolume", "PromptVolume",
|
||||
"PromptDistractedVolume", "RefuseVolume", "WarningImmediateVolume", "WarningSoftVolume",
|
||||
}:
|
||||
_, value_types = _get_param_type_info()
|
||||
updated[key] = _get_current_param_value(key, value_types.get(key, int), _get_default_param_values())
|
||||
if key in PANDA_FIRMWARE_TOGGLE_KEYS:
|
||||
threading.Thread(target=_flash_panda_then_reboot, daemon=True).start()
|
||||
response["message"] = f"Parameter '{key}' updated successfully. Panda flashing started; device will reboot when finished."
|
||||
|
||||
Reference in New Issue
Block a user