diff --git a/openpilot/selfdrive/controls/controlsd.py b/openpilot/selfdrive/controls/controlsd.py index c81ed08702..81499a496b 100755 --- a/openpilot/selfdrive/controls/controlsd.py +++ b/openpilot/selfdrive/controls/controlsd.py @@ -493,11 +493,7 @@ class Controls: new_desired_curvature = self.model_turn_assist.update(CS, model_v2, CC, new_desired_curvature) elif getattr(self, 'model_turn_assist', None) is not None: self.model_turn_assist.reset() -<<<<<<< Updated upstream - self.model_turn_assist.twitch_guard_remaining = update_twitch_guard(0.0, CS.vEgo, CS.standstill) -======= self.model_turn_assist.twitch_guard_armed = update_twitch_guard(False, CS.vEgo, CS.standstill) ->>>>>>> Stashed changes lane_base_curvature = new_desired_curvature self.lane_centering_applied = 0.0 self.lane_centering_inactive_reason = 'inactive' diff --git a/openpilot/starpilot/lateral/model_turn_assist.py b/openpilot/starpilot/lateral/model_turn_assist.py index 016dc260b8..fe99c58dae 100644 --- a/openpilot/starpilot/lateral/model_turn_assist.py +++ b/openpilot/starpilot/lateral/model_turn_assist.py @@ -125,25 +125,14 @@ def update_twitch_guard(armed: bool, v_ego: float, standstill: bool) -> bool: if not math.isfinite(v_ego): return False if standstill or abs(v_ego) <= 0.3: -<<<<<<< Updated upstream - return TWITCH_GUARD_DURATION - return max(remaining - DT_CTRL, 0.0) -<<<<<<< Updated upstream -======= -======= return True return armed and v_ego < TWITCH_GUARD_MAX_SPEED ->>>>>>> Stashed changes def stopping_distance(v_ego: float, a_ego: float) -> float: if not (math.isfinite(v_ego) and math.isfinite(a_ego)) or v_ego >= STOP_HOLD_MAX_SPEED: return math.inf return v_ego ** 2 / (2.0 * max(-a_ego, 0.01)) -<<<<<<< Updated upstream -======= ->>>>>>> Stashed changes ->>>>>>> Stashed changes TURN_LEAD_T = 1.3 TURN_LEAD_MIN_M = 4.0 TURN_LEAD_MAX_M = 14.0 @@ -169,39 +158,13 @@ class ModelTurnAssist: self.turn_hold_swept = 0.0 self.turn_hold_handoff_t = 0.0 self.turn_blinker_swept = 0.0 -<<<<<<< Updated upstream - self.twitch_guard_remaining = 0.0 -<<<<<<< Updated upstream - self.stop_hold_curvature = None -======= -======= self.twitch_guard_armed = False self.stop_hold_curvature = None ->>>>>>> Stashed changes ->>>>>>> Stashed changes self.curvature = 0.0 self.turn_hold_done = False def update(self, CS, model_v2, CC, new_desired_curvature): blinker_dir = float(CS.rightBlinker) - float(CS.leftBlinker) -<<<<<<< Updated upstream - stop_dist = stopping_distance(CS.vEgo, CS.aEgo) - entering = CS.aEgo < -STOP_HOLD_MIN_DECEL and stop_dist < STOP_HOLD_DIST - holding = self.stop_hold_curvature is not None and stop_dist < STOP_HOLD_RELEASE_DIST - if CC.latActive and blinker_dir == 0.0 and self.turn_hold_curvature == 0.0 and (entering or holding): - if CS.steeringPressed: - self.stop_hold_curvature = self.curvature - elif self.stop_hold_curvature is None: - self.stop_hold_curvature = new_desired_curvature - new_desired_curvature = self.stop_hold_curvature - else: - self.stop_hold_curvature = None -======= -<<<<<<< Updated upstream ->>>>>>> Stashed changes - if CC.latActive and self.twitch_guard_remaining > 0.0 and (blinker_dir == 0.0) and (self.turn_hold_curvature == 0.0): - new_desired_curvature = limit_curvature_to_plan(model_v2, new_desired_curvature, CS.vEgo) -======= stop_dist = stopping_distance(CS.vEgo, CS.aEgo) entering = CS.aEgo < -STOP_HOLD_MIN_DECEL and stop_dist < STOP_HOLD_DIST holding = self.stop_hold_curvature is not None and stop_dist < STOP_HOLD_RELEASE_DIST @@ -216,7 +179,6 @@ class ModelTurnAssist: self.stop_hold_curvature = None if CC.latActive and self.twitch_guard_armed and (blinker_dir == 0.0) and (self.turn_hold_curvature == 0.0): new_desired_curvature = limit_curvature_to_plan(model_v2, new_desired_curvature, CS.vEgo) ->>>>>>> Stashed changes if blinker_dir == 0.0: self.turn_blinker_swept = 0.0 else: diff --git a/openpilot/starpilot/lateral/tests/test_model_turn_assist_stop_hold.py b/openpilot/starpilot/lateral/tests/test_model_turn_assist_stop_hold.py index 459a2d11e9..6fc98aa730 100644 --- a/openpilot/starpilot/lateral/tests/test_model_turn_assist_stop_hold.py +++ b/openpilot/starpilot/lateral/tests/test_model_turn_assist_stop_hold.py @@ -1,12 +1,6 @@ import unittest from types import SimpleNamespace -<<<<<<< Updated upstream -from openpilot.starpilot.lateral.model_turn_assist import STOP_HOLD_MAX_SPEED, ModelTurnAssist - -STRAIGHT = SimpleNamespace(position=SimpleNamespace(x=[0., 1., 2., 3.], y=[0., 0., 0., 0.]), - meta=SimpleNamespace(laneChangeState=0)) -======= from openpilot.starpilot.lateral.model_turn_assist import ( STOP_HOLD_MAX_SPEED, TWITCH_GUARD_FLOOR, TWITCH_GUARD_MAX_SPEED, ModelTurnAssist, update_twitch_guard, ) @@ -15,7 +9,6 @@ STRAIGHT = SimpleNamespace(position=SimpleNamespace(x=[0., 1., 2., 3.], y=[0., 0 meta=SimpleNamespace(laneChangeState=0)) LONG_STRAIGHT = SimpleNamespace(position=SimpleNamespace(x=[float(x) for x in range(0, 80, 4)], y=[0.] * 20), meta=SimpleNamespace(laneChangeState=0)) ->>>>>>> Stashed changes def car(speed, accel, blinker=0, pressed=False): @@ -23,13 +16,8 @@ def car(speed, accel, blinker=0, pressed=False): steeringPressed=pressed, steeringTorque=0.) -<<<<<<< Updated upstream -def step(assist, speed, accel, desired, active=True, **kwargs): - return assist.update(car(speed, accel, **kwargs), STRAIGHT, SimpleNamespace(latActive=active), desired) -======= def step(assist, speed, accel, desired, active=True, model=STRAIGHT, **kwargs): return assist.update(car(speed, accel, **kwargs), model, SimpleNamespace(latActive=active), desired) ->>>>>>> Stashed changes class TestStopHold(unittest.TestCase): @@ -72,8 +60,6 @@ class TestStopHold(unittest.TestCase): self.assertEqual(step(assist, .6, -2.0, .015), -.003) -<<<<<<< Updated upstream -======= class TestTwitchGuardArming(unittest.TestCase): def test_armed_from_standstill_until_max_speed(self): self.assertTrue(update_twitch_guard(False, 0., True)) @@ -95,6 +81,5 @@ class TestTwitchGuardArming(unittest.TestCase): self.assertEqual(step(assist, 1.0, 0., .012, model=LONG_STRAIGHT), .012) ->>>>>>> Stashed changes if __name__ == '__main__': unittest.main()