Skip to content

Keep steering-limit feedback current during Always On Lateral #166

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Intelli wants to merge 1 commit into
base: Dom
Choose a base branch
from
+43 −1
Open

Keep steering-limit feedback current during Always On Lateral #166

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 3 additions & 1 deletion selfdrive/controls/controlsd.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -875,7 +875,7 @@ def publish(self, CC, lac_log):
hudControl.leftLaneDepart = self.sm['driverAssistance'].leftLaneDeparture
hudControl.rightLaneDepart = self.sm['driverAssistance'].rightLaneDeparture

if self.sm['selfdriveState'].active:
if CC.latActive:
CO = self.sm['carOutput']
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
output_healthy = (
Expand All @@ -889,6 +889,8 @@ def publish(self, CC, lac_log):
)
else:
self.steer_limited_by_safety = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2
else:
self.steer_limited_by_safety = False

# TODO: both controlsState and carControl valids should be set by
# sm.all_checks(), but this creates a circular dependency
Expand Down
40 changes: 40 additions & 0 deletions selfdrive/controls/tests/test_steering_saturation_publish.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -160,3 +160,43 @@ def test_replay_genuine_limiter_error_remains_visible(monkeypatch):
controls = run_publish(monkeypatch, True, FRESH_TIME_NANOS, real_limit_error=3.0)

assert controls.steer_limited_by_safety


@pytest.mark.parametrize("steer_control_type", ("angle", "torque"))
def test_feedback_follows_lateral_activity(monkeypatch, steer_control_type):
monkeypatch.setattr(controlsd, "REPLAY", True)
controls = make_controls(make_car_output(), make_starpilot_car_control(), FRESH_TIME_NANOS)
cp = controls.CP.as_builder()
cp.steerControlType = steer_control_type
if steer_control_type == "torque":
cp.lateralTuning.init("torque")
controls.CP = cp.as_reader()

cc = car.CarControl.new_message()
cc.actuators.steeringAngleDeg = REQUESTED_ANGLE
cc.actuators.torque = 0.5
lac_log = (log.ControlsState.LateralAngleState if steer_control_type == "angle" else log.ControlsState.LateralTorqueState).new_message()

# AOL must track both a new limit and its removal without normal engagement.
# Stopping lateral control must clear feedback even if normal engagement remains active.
for active, lat_active, limited in (
(False, True, True),
(False, True, False),
(True, True, True),
(False, False, True),
(True, True, True),
(True, False, True),
(True, True, False),
):
controls.sm.messages["selfdriveState"] = log.SelfdriveState.new_message(active=active).as_reader()
output = make_car_output()
output.actuatorsOutput.steeringAngleDeg = OUTPUT_ANGLE if limited else REQUESTED_ANGLE
output.actuatorsOutput.torque = 0.0 if limited else cc.actuators.torque
controls.sm.messages["carOutput"] = output.as_reader()
controls.sm.messages["starpilotCarControl"] = make_starpilot_car_control(real_limit_error=3.0 if limited else 0.0).as_reader()
cc.enabled = active
cc.latActive = lat_active

controls.publish(cc, lac_log)

assert controls.steer_limited_by_safety is (lat_active and limited), (active, lat_active, limited)