diff --git a/common/params_keys.h b/common/params_keys.h index 7cfdf7540..89f70471c 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -224,6 +224,7 @@ inline static std::unordered_map keys = { {"LagdValueCache", {PERSISTENT, FLOAT, "0.2"}}, {"LaneTurnDesire", {PERSISTENT | BACKUP, BOOL, "0"}}, {"LaneTurnValue", {PERSISTENT | BACKUP, FLOAT, "19.0"}}, + {"PlanplusControl", {PERSISTENT | BACKUP, FLOAT, "1.0"}}, // mapd {"MapAdvisorySpeedLimit", {CLEAR_ON_ONROAD_TRANSITION, FLOAT}}, diff --git a/sunnypilot/modeld_v2/modeld.py b/sunnypilot/modeld_v2/modeld.py index 82eb099e7..b3b1d35fd 100755 --- a/sunnypilot/modeld_v2/modeld.py +++ b/sunnypilot/modeld_v2/modeld.py @@ -28,7 +28,6 @@ from openpilot.sunnypilot.models.helpers import get_active_bundle from openpilot.sunnypilot.models.runners.helpers import get_model_runner PROCESS_NAME = "selfdrive.modeld.modeld_tinygrad" -RECOVERY_POWER = 1.0 # The higher this number the more aggressively the model will recover to lanecenter, too high and it will ping-pong class FrameMeta: @@ -63,6 +62,7 @@ class ModelState(ModelStateBase): self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".0")) self.LONG_SMOOTH_SECONDS = float(overrides.get('long', ".0")) self.MIN_LAT_CONTROL_SPEED = 0.3 + self.PLANPLUS_CONTROL: float = 1.0 buffer_length = 5 if self.model_runner.is_20hz else 2 self.frames = {name: DrivingModelFrame(context, buffer_length) for name in self.model_runner.vision_input_names} @@ -158,7 +158,8 @@ class ModelState(ModelStateBase): lat_action_t: float, long_action_t: float, v_ego: float) -> log.ModelDataV2.Action: plan = model_output['plan'][0] if 'planplus' in model_output: - plan = plan + RECOVERY_POWER*model_output['planplus'][0] + recovery_power = self.PLANPLUS_CONTROL * (0.75 if v_ego > 20.0 else 1.0) + plan = plan + recovery_power * model_output['planplus'][0] desired_accel, should_stop = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], self.constants.T_IDXS, action_t=long_action_t) desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS) @@ -283,6 +284,7 @@ def main(demo=False): v_ego = max(sm["carState"].vEgo, 0.) if sm.frame % 60 == 0: model.lat_delay = get_lat_delay(params, sm["liveDelay"].lateralDelay) + model.PLANPLUS_CONTROL = params.get("PlanplusControl", return_default=True) lat_delay = model.lat_delay + model.LAT_SMOOTH_SECONDS if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) diff --git a/sunnypilot/modeld_v2/tests/test_recovery_power.py b/sunnypilot/modeld_v2/tests/test_recovery_power.py new file mode 100644 index 000000000..e38b2c86d --- /dev/null +++ b/sunnypilot/modeld_v2/tests/test_recovery_power.py @@ -0,0 +1,61 @@ +import numpy as np + +from cereal import log + +from openpilot.sunnypilot.modeld_v2.constants import Plan +from openpilot.sunnypilot.modeld_v2.modeld import ModelState +import openpilot.sunnypilot.modeld_v2.modeld as modeld + + +class MockStruct: + def __init__(self, **kwargs): + for k, v in kwargs.items(): + setattr(self, k, v) + + +def test_recovery_power_scaling(): + state = MockStruct( + PLANPLUS_CONTROL=1.0, + LONG_SMOOTH_SECONDS=0.3, + LAT_SMOOTH_SECONDS=0.1, + MIN_LAT_CONTROL_SPEED=0.3, + mlsim=True, + generation=12, + constants=MockStruct(T_IDXS=np.arange(100), DESIRE_LEN=8) + ) + prev_action = log.ModelDataV2.Action() + recorded_vel: list = [] + + def mock_accel(plan_vel, plan_accel, t_idxs, action_t=0.0): + recorded_vel.append(plan_vel.copy()) + return 0.0, False + + modeld.get_accel_from_plan = mock_accel + modeld.get_curvature_from_output = lambda *args: 0.0 + plan = np.random.rand(1, 100, 15).astype(np.float32) + planplus = np.random.rand(1, 100, 15).astype(np.float32) + + model_output: dict = { + 'plan': plan.copy(), + 'planplus': planplus.copy() + } + + test_cases: list = [ + # (control, v_ego, expected_factor) + (0.55, 20.0, 1.0), + (1.0, 25.0, .75), + (1.5, 25.1, 0.75), + (2.0, 20.0, 1.0), + (0.75, 19.0, 1.0), + (0.8, 25.1, 0.75), + ] + + for control, v_ego, factor in test_cases: + state.PLANPLUS_CONTROL = control + recorded_vel.clear() + ModelState.get_action_from_model(state, model_output, prev_action, 0.0, 0.0, v_ego) + + expected_recovery_power = control * factor + expected_plan_vel = plan[0, :, Plan.VELOCITY][:, 0] + expected_recovery_power * planplus[0, :, Plan.VELOCITY][:, 0] + + np.testing.assert_allclose(recorded_vel[0], expected_plan_vel, rtol=1e-5, atol=1e-6) diff --git a/sunnypilot/sunnylink/params_metadata.json b/sunnypilot/sunnylink/params_metadata.json index bbefc3d0c..f335457fb 100644 --- a/sunnypilot/sunnylink/params_metadata.json +++ b/sunnypilot/sunnylink/params_metadata.json @@ -826,6 +826,13 @@ "title": "Panda Som Reset Triggered", "description": "" }, + "PlanplusControl": { + "title": "Plan Plus Controls", + "description": "Adjust planplus model recentering strength. The higher this number the more aggressively the model will recover to lanecenter, too high and it will ping-pong", + "min": 0.0, + "max": 2.0, + "step": 0.1 + }, "PrimeType": { "title": "Prime Type", "description": ""