diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 1f14636bdd..908dee7d56 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -14,6 +14,7 @@ using Car = import "car.capnp"; struct StarPilotCarControl @0x81c2f05a394cf4af { hudControl @0 :HUDControl; + steeringLimitInfo @1 :SteeringLimitInfo; struct HUDControl { audibleAlert @0 :AudibleAlert; @@ -49,6 +50,16 @@ struct StarPilotCarControl @0x81c2f05a394cf4af { uwu @22; } } + + struct SteeringLimitInfo { + valid @0 :Bool; + modelLimitErrorDeg @1 :Float32; + resumeLimitErrorDeg @2 :Float32; + cooperativeLimitErrorDeg @3 :Float32; + cooperativeOffsetDeg @4 :Float32; + monoTime @5 :UInt64; + combinedLimitErrorDeg @6 :Float32; + } } struct StarPilotCarParams @0xaedffd8f31e7b55d { diff --git a/cereal/libcereal.a b/cereal/libcereal.a index 9bdac9f8d1..a242c1f0f8 100644 Binary files a/cereal/libcereal.a and b/cereal/libcereal.a differ diff --git a/opendbc_repo/opendbc/car/car.capnp b/opendbc_repo/opendbc/car/car.capnp index bf205202d1..a0d3fad8a4 100644 --- a/opendbc_repo/opendbc/car/car.capnp +++ b/opendbc_repo/opendbc/car/car.capnp @@ -375,17 +375,6 @@ struct CarControl { torqueOutputCan @8: Float32; # value sent over can to the car speed @6: Float32; # m/s lateralControlMode @9: LateralControlMode; - steeringLimitInfo @10 :SteeringLimitInfo; - - struct SteeringLimitInfo { - valid @0 :Bool; - modelLimitErrorDeg @1 :Float32; - resumeLimitErrorDeg @2 :Float32; - cooperativeLimitErrorDeg @3 :Float32; - cooperativeOffsetDeg @4 :Float32; - monoTime @5 :UInt64; - combinedLimitErrorDeg @6 :Float32; - } enum LongControlState @0xe40f3a917d908282{ off @0; diff --git a/opendbc_repo/opendbc/car/tesla/carcontroller.py b/opendbc_repo/opendbc/car/tesla/carcontroller.py index d4aab08077..c29a836fba 100644 --- a/opendbc_repo/opendbc/car/tesla/carcontroller.py +++ b/opendbc_repo/opendbc/car/tesla/carcontroller.py @@ -49,15 +49,16 @@ class CarController(CarControllerBase): self.steering_limit_mono_time = 0 self.combined_limit_error_deg = 0.0 - def _write_steering_limit_info(self, actuators): - info = actuators.steeringLimitInfo - info.valid = self.steering_limit_info_valid - info.modelLimitErrorDeg = self.model_limit_error_deg - info.resumeLimitErrorDeg = self.resume_limit_error_deg - info.cooperativeLimitErrorDeg = self.cooperative_limit_error_deg - info.cooperativeOffsetDeg = self.cooperative_offset_deg - info.monoTime = self.steering_limit_mono_time - info.combinedLimitErrorDeg = self.combined_limit_error_deg + def get_steering_limit_info(self) -> dict[str, bool | float | int]: + return { + "valid": self.steering_limit_info_valid, + "modelLimitErrorDeg": self.model_limit_error_deg, + "resumeLimitErrorDeg": self.resume_limit_error_deg, + "cooperativeLimitErrorDeg": self.cooperative_limit_error_deg, + "cooperativeOffsetDeg": self.cooperative_offset_deg, + "monoTime": self.steering_limit_mono_time, + "combinedLimitErrorDeg": self.combined_limit_error_deg, + } def update(self, CC, CS, now_nanos, starpilot_toggles): if self.CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP: @@ -125,7 +126,6 @@ class CarController(CarControllerBase): # TODO: HUD control new_actuators = actuators.as_builder() new_actuators.steeringAngleDeg = self.apply_angle_command_last - self._write_steering_limit_info(new_actuators) self.frame += 1 return new_actuators, can_sends @@ -168,7 +168,6 @@ class CarController(CarControllerBase): new_actuators = actuators.as_builder() new_actuators.steeringAngleDeg = self.apply_angle_last - self._write_steering_limit_info(new_actuators) self.frame += 1 return new_actuators, can_sends diff --git a/opendbc_repo/opendbc/car/tesla/tests/test_coop_steering_limit_info.py b/opendbc_repo/opendbc/car/tesla/tests/test_coop_steering_limit_info.py index 05ea35da2d..3515401f50 100644 --- a/opendbc_repo/opendbc/car/tesla/tests/test_coop_steering_limit_info.py +++ b/opendbc_repo/opendbc/car/tesla/tests/test_coop_steering_limit_info.py @@ -5,11 +5,13 @@ from types import SimpleNamespace import pytest +import cereal.messaging as messaging + from cereal import car from opendbc.car import gen_empty_fingerprint from opendbc.car.tesla.carcontroller import CarController from opendbc.car.tesla.interface import CarInterface -from opendbc.car.tesla.values import CAR, DBC, TeslaSafetyFlags +from opendbc.car.tesla.values import CAR, DBC BASELINE_SHA = "a80064be4fdf4b8765a5e9dec44d8a5c266f48a8" @@ -62,21 +64,25 @@ def run_frame(controller, requested_angle=0.0, torque=0.0, speed=15.0, measured_ ) +def get_limit_info(controller): + return SimpleNamespace(**controller.get_steering_limit_info()) + + def legacy_actuator_dict(actuators): - values = actuators.to_dict() - values.pop("steeringLimitInfo", None) - return values + return actuators.to_dict() def test_steering_limit_info_defaults_to_invalid(): - actuators = car.CarControl.Actuators.new_message() + controller = make_controller() - assert not actuators.steeringLimitInfo.valid + info = get_limit_info(controller) + assert not info.valid + assert info.monoTime == 0 -def test_steering_limit_info_round_trips_through_car_output(): - output = car.CarOutput.new_message() - info = output.actuatorsOutput.steeringLimitInfo +def test_steering_limit_info_round_trips_through_custom_message(): + message = messaging.new_message("starpilotCarControl", valid=True) + info = message.starpilotCarControl.steeringLimitInfo info.valid = True info.modelLimitErrorDeg = 1.25 info.resumeLimitErrorDeg = 0.5 @@ -85,25 +91,25 @@ def test_steering_limit_info_round_trips_through_car_output(): info.monoTime = 1_234_567_890 info.combinedLimitErrorDeg = 3.75 - with car.CarOutput.from_bytes(output.to_bytes()) as restored: - restored_info = restored.actuatorsOutput.steeringLimitInfo - assert restored_info.valid - assert restored_info.modelLimitErrorDeg == 1.25 - assert restored_info.resumeLimitErrorDeg == 0.5 - assert restored_info.cooperativeLimitErrorDeg == 2.0 - assert restored_info.cooperativeOffsetDeg == -4.5 - assert restored_info.monoTime == 1_234_567_890 - assert restored_info.combinedLimitErrorDeg == 3.75 + restored = messaging.log_from_bytes(message.to_bytes()) + restored_info = restored.starpilotCarControl.steeringLimitInfo + assert restored_info.valid + assert restored_info.modelLimitErrorDeg == 1.25 + assert restored_info.resumeLimitErrorDeg == 0.5 + assert restored_info.cooperativeLimitErrorDeg == 2.0 + assert restored_info.cooperativeOffsetDeg == -4.5 + assert restored_info.monoTime == 1_234_567_890 + assert restored_info.combinedLimitErrorDeg == 3.75 -def test_active_cooperative_controller_publishes_r_n_a_t_f_diagnostics(): +def test_active_cooperative_controller_reports_diagnostics(): controller = make_controller() requested_angle = 20.0 now_nanos = 1_234_567_890 actuators, _ = run_frame(controller, requested_angle, torque=0.9, measured_angle=0.0, now_nanos=now_nanos) - info = actuators.steeringLimitInfo + info = get_limit_info(controller) assert info.valid assert info.monoTime == now_nanos assert info.modelLimitErrorDeg == pytest.approx(abs(requested_angle - controller.apply_angle_last), abs=1e-5) @@ -129,7 +135,7 @@ def test_cooperative_offset_alone_does_not_become_limiter_error(): actuators, _ = run_frame(controller, torque=0.9, now_nanos=1_000_000_000 + frame * 10_000_000) assert actuators is not None - info = actuators.steeringLimitInfo + info = get_limit_info(controller) assert info.valid assert info.cooperativeOffsetDeg > 2.5 assert info.modelLimitErrorDeg < 2.5 @@ -145,7 +151,7 @@ def test_combined_error_keeps_two_same_direction_small_limits_visible(): actuators, _ = run_frame(controller, -2.5, torque=-1.5, speed=12.5, measured_angle=0.0) - info = actuators.steeringLimitInfo + info = get_limit_info(controller) assert info.modelLimitErrorDeg == pytest.approx(1.5045133, abs=1e-5) assert info.resumeLimitErrorDeg == pytest.approx(0.0, abs=1e-5) assert info.cooperativeLimitErrorDeg == pytest.approx(1.5045133, abs=1e-5) @@ -158,26 +164,26 @@ def test_combined_error_keeps_two_same_direction_small_limits_visible(): def test_intervening_100hz_frame_retains_matching_50hz_sample(): controller = make_controller() first, _ = run_frame(controller, 8.0, torque=0.9, now_nanos=1_000_000_000) - first_info = first.steeringLimitInfo.to_dict() + first_info = controller.get_steering_limit_info() second, _ = run_frame(controller, -40.0, torque=-1.5, now_nanos=1_010_000_000) - assert second.steeringLimitInfo.to_dict() == first_info - assert second.steeringLimitInfo.monoTime == 1_000_000_000 + assert controller.get_steering_limit_info() == first_info + assert controller.get_steering_limit_info()["monoTime"] == 1_000_000_000 def test_inactive_interval_clears_sample_until_next_steering_update(): controller = make_controller() active, _ = run_frame(controller, 8.0, torque=0.9, now_nanos=1_000_000_000) - assert active.steeringLimitInfo.valid + assert get_limit_info(controller).valid inactive, _ = run_frame(controller, 8.0, torque=0.9, lat_active=False, now_nanos=1_010_000_000) - assert not inactive.steeringLimitInfo.valid - assert inactive.steeringLimitInfo.monoTime == 0 + assert not get_limit_info(controller).valid + assert get_limit_info(controller).monoTime == 0 resumed, _ = run_frame(controller, 8.0, torque=0.9, now_nanos=1_020_000_000) - assert resumed.steeringLimitInfo.valid - assert resumed.steeringLimitInfo.monoTime == 1_020_000_000 + assert get_limit_info(controller).valid + assert get_limit_info(controller).monoTime == 1_020_000_000 @pytest.mark.parametrize(("candidate", "cooperative", "steering_disengage"), ( @@ -190,8 +196,9 @@ def test_diagnostics_invalid_when_not_in_supported_active_path(candidate, cooper actuators, _ = run_frame(controller, torque=1.5, steering_disengage=steering_disengage) - assert not actuators.steeringLimitInfo.valid - assert actuators.steeringLimitInfo.monoTime == 0 + info = get_limit_info(controller) + assert not info.valid + assert info.monoTime == 0 def test_actual_actuators_and_steering_can_match_pinned_baseline_fixture(): diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 2e83ce373d..9e2d1a4e8d 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -43,6 +43,22 @@ REDNECK_DECREASE_LOOKAHEAD_POINTS = 10 SLC_SOURCE_NONE = "None" EventName = log.OnroadEvent.EventName + +def _build_starpilot_car_control(steering_limit_info: dict[str, bool | float | int] | None, valid: bool): + message = messaging.new_message('starpilotCarControl') + message.valid = valid + if steering_limit_info is not None: + info = message.starpilotCarControl.steeringLimitInfo + info.valid = bool(steering_limit_info.get("valid", False)) + info.modelLimitErrorDeg = float(steering_limit_info.get("modelLimitErrorDeg", 0.0)) + info.resumeLimitErrorDeg = float(steering_limit_info.get("resumeLimitErrorDeg", 0.0)) + info.cooperativeLimitErrorDeg = float(steering_limit_info.get("cooperativeLimitErrorDeg", 0.0)) + info.cooperativeOffsetDeg = float(steering_limit_info.get("cooperativeOffsetDeg", 0.0)) + info.monoTime = int(steering_limit_info.get("monoTime", 0)) + info.combinedLimitErrorDeg = float(steering_limit_info.get("combinedLimitErrorDeg", 0.0)) + return message + + # forward carlog.addHandler(ForwardingHandler(cloudlog)) @@ -86,7 +102,7 @@ class Car: def __init__(self, CI=None, RI=None) -> None: self.can_sock = messaging.sub_sock('can', timeout=20) self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents', 'radarState', 'longitudinalPlan']) - self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks']) + self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks', 'starpilotCarControl']) self.gps_pm = None self.can_rcv_cum_timeout_counter = 0 @@ -99,6 +115,7 @@ class Car: self.initialized_prev = False self.last_actuators_output = structs.CarControl.Actuators() + self.last_steering_limit_info: dict[str, bool | float | int] | None = None self.params = Params() self.params_memory = Params(memory=True) @@ -396,6 +413,12 @@ class Car: co_send.carOutput.actuatorsOutput = self.last_actuators_output self.pm.send('carOutput', co_send) + starpilot_control_send = _build_starpilot_car_control( + self.last_steering_limit_info, + CS.canValid and self.sm.all_checks(['carControl']), + ) + self.pm.send('starpilotCarControl', starpilot_control_send) + # kick off controlsd step while we actuate the latest carControl packet cs_send = messaging.new_message('carState') cs_send.valid = CS.canValid @@ -450,6 +473,8 @@ class Car: self.CI.CC.update_live_params(live_params.roll, live_params.angleOffsetDeg, live_params.stiffnessFactor, live_params.steerRatio) self.last_actuators_output, can_sends = self.CI.apply(CC, now_nanos, self.starpilot_toggles) + get_steering_limit_info = getattr(self.CI.CC, "get_steering_limit_info", None) + self.last_steering_limit_info = get_steering_limit_info() if get_steering_limit_info is not None else None self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) self.CC_prev = CC diff --git a/selfdrive/car/tests/test_starpilot_car_control.py b/selfdrive/car/tests/test_starpilot_car_control.py new file mode 100644 index 0000000000..9bf54b12bc --- /dev/null +++ b/selfdrive/car/tests/test_starpilot_car_control.py @@ -0,0 +1,30 @@ +import cereal.messaging as messaging + +from cereal import car +from openpilot.selfdrive.car.card import _build_starpilot_car_control + + +def test_steering_diagnostics_use_reserved_custom_message(): + values = { + "valid": True, + "modelLimitErrorDeg": 1.25, + "resumeLimitErrorDeg": 0.5, + "cooperativeLimitErrorDeg": 2.0, + "cooperativeOffsetDeg": -4.5, + "monoTime": 1_234_567_890, + "combinedLimitErrorDeg": 3.75, + } + + message = _build_starpilot_car_control(values, True) + restored = messaging.log_from_bytes(message.to_bytes()) + info = restored.starpilotCarControl.steeringLimitInfo + + assert restored.valid + for field, value in values.items(): + assert getattr(info, field) == value + + +def test_stock_car_output_has_no_fork_specific_fields(): + actuators = car.CarOutput.new_message().actuatorsOutput + + assert "steeringLimitInfo" not in actuators.to_dict() diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 94363186d4..0fdf7c97cb 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -390,6 +390,7 @@ class Controls: self.sm = messaging.SubMaster(['liveDelay', 'liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState', 'liveCalibration', 'livePose', 'longitudinalPlan', 'lateralManeuverPlan', 'carState', 'carOutput', + 'starpilotCarControl', 'driverMonitoringState', 'onroadEvents', 'driverAssistance', 'radarState'], poll='selfdriveState') self.pm = messaging.PubMaster(['carControl', 'controlsState', 'starpilotLateralState']) @@ -884,7 +885,7 @@ class Controls: ) now_nanos = self.sm.logMonoTime['selfdriveState'] if REPLAY else time.monotonic_ns() self.steer_limited_by_safety = is_angle_steering_limited( - self.CP, CC.actuators.steeringAngleDeg, CO, output_healthy, now_nanos, + self.CP, CC.actuators.steeringAngleDeg, CO, self.sm['starpilotCarControl'], output_healthy, now_nanos, ) else: self.steer_limited_by_safety = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2 diff --git a/selfdrive/controls/lib/steering_saturation.py b/selfdrive/controls/lib/steering_saturation.py index 827527a3a4..b2da8b04cf 100644 --- a/selfdrive/controls/lib/steering_saturation.py +++ b/selfdrive/controls/lib/steering_saturation.py @@ -3,8 +3,7 @@ import math from opendbc.car.tesla.values import CAR, TeslaSafetyFlags -# TODO This is speed dependent -STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees +STEER_ANGLE_SATURATION_THRESHOLD = 2.5 _MAX_STEERING_LIMIT_INFO_AGE_NANOS = 100_000_000 @@ -13,7 +12,8 @@ def _legacy_angle_steering_limited(requested_angle: float, car_output) -> bool: STEER_ANGLE_SATURATION_THRESHOLD -def is_angle_steering_limited(CP, requested_angle: float, car_output, output_healthy: bool, now_nanos: int) -> bool: +def is_angle_steering_limited(CP, requested_angle: float, car_output, starpilot_car_control, + output_healthy: bool, now_nanos: int) -> bool: legacy_limited = _legacy_angle_steering_limited(requested_angle, car_output) try: @@ -24,7 +24,10 @@ def is_angle_steering_limited(CP, requested_angle: float, car_output, output_hea if CP.carFingerprint != CAR.TESLA_MODEL_3 or not cooperative_enabled or not output_healthy: return legacy_limited - info = car_output.actuatorsOutput.steeringLimitInfo + if not starpilot_car_control.valid: + return legacy_limited + + info = starpilot_car_control.starpilotCarControl.steeringLimitInfo errors = ( info.modelLimitErrorDeg, info.resumeLimitErrorDeg, diff --git a/selfdrive/controls/tests/test_steering_saturation.py b/selfdrive/controls/tests/test_steering_saturation.py index 520b74e614..c7425c79b8 100644 --- a/selfdrive/controls/tests/test_steering_saturation.py +++ b/selfdrive/controls/tests/test_steering_saturation.py @@ -1,5 +1,6 @@ import math +import cereal.messaging as messaging import pytest from cereal import car @@ -33,39 +34,44 @@ def make_case(candidate=CAR.TESLA_MODEL_3, cooperative_enabled=True): output = car.CarOutput.new_message() output.actuatorsOutput.steeringAngleDeg = OUTPUT_ANGLE - info = output.actuatorsOutput.steeringLimitInfo + + diagnostics = messaging.new_message("starpilotCarControl", valid=True) + info = diagnostics.starpilotCarControl.steeringLimitInfo info.valid = True info.monoTime = SAMPLE_TIME_NANOS info.cooperativeOffsetDeg = 5.5 - return cp, output + return cp, output, diagnostics -def detect(cp, output, requested_angle=REQUESTED_ANGLE, output_healthy=True, now_nanos=FRESH_TIME_NANOS): - return is_angle_steering_limited(cp.as_reader(), requested_angle, output.as_reader(), output_healthy, now_nanos) +def detect(cp, output, diagnostics, requested_angle=REQUESTED_ANGLE, + output_healthy=True, now_nanos=FRESH_TIME_NANOS): + return is_angle_steering_limited( + cp.as_reader(), requested_angle, output.as_reader(), diagnostics.as_reader(), output_healthy, now_nanos, + ) def test_light_offset_does_not_count_as_limiting(): - cp, output = make_case() + cp, output, diagnostics = make_case() - assert not detect(cp, output) + assert not detect(cp, output, diagnostics) @pytest.mark.parametrize("field", ERROR_FIELDS) def test_each_genuine_limit_is_visible_during_cooperation(field): - cp, output = make_case() - setattr(output.actuatorsOutput.steeringLimitInfo, field, 3.0) + cp, output, diagnostics = make_case() + setattr(diagnostics.starpilotCarControl.steeringLimitInfo, field, 3.0) - assert detect(cp, output) + assert detect(cp, output, diagnostics) def test_combined_small_limits_remain_visible(): - cp, output = make_case() - info = output.actuatorsOutput.steeringLimitInfo + cp, output, diagnostics = make_case() + info = diagnostics.starpilotCarControl.steeringLimitInfo info.modelLimitErrorDeg = 1.5 info.cooperativeLimitErrorDeg = 1.5 info.combinedLimitErrorDeg = 3.0 - assert detect(cp, output) + assert detect(cp, output, diagnostics) @pytest.mark.parametrize(("error", "expected"), ( @@ -73,24 +79,24 @@ def test_combined_small_limits_remain_visible(): (NEXT_FLOAT32_AFTER_2_5, True), )) def test_diagnostic_threshold_is_strictly_greater(error, expected): - cp, output = make_case() - output.actuatorsOutput.steeringLimitInfo.modelLimitErrorDeg = error + cp, output, diagnostics = make_case() + diagnostics.starpilotCarControl.steeringLimitInfo.modelLimitErrorDeg = error - assert detect(cp, output) is expected + assert detect(cp, output, diagnostics) is expected @pytest.mark.parametrize("age_nanos", (0, MAX_DIAGNOSTIC_AGE_NANOS)) def test_diagnostic_age_bounds_are_inclusive(age_nanos): - cp, output = make_case() + cp, output, diagnostics = make_case() - assert not detect(cp, output, now_nanos=SAMPLE_TIME_NANOS + age_nanos) + assert not detect(cp, output, diagnostics, now_nanos=SAMPLE_TIME_NANOS + age_nanos) def test_signed_cooperative_offset_is_valid_data(): - cp, output = make_case() - output.actuatorsOutput.steeringLimitInfo.cooperativeOffsetDeg = -5.5 + cp, output, diagnostics = make_case() + diagnostics.starpilotCarControl.steeringLimitInfo.cooperativeOffsetDeg = -5.5 - assert not detect(cp, output) + assert not detect(cp, output, diagnostics) @pytest.mark.parametrize("scenario", ( @@ -104,29 +110,28 @@ def test_signed_cooperative_offset_is_valid_data(): "cooperative_mode_disabled", )) def test_unusable_diagnostics_keep_legacy_warning(scenario): - cp, output = make_case() + cp, output, diagnostics = make_case() output_healthy = True now_nanos = FRESH_TIME_NANOS if scenario == "default_message": - output = car.CarOutput.new_message() - output.actuatorsOutput.steeringAngleDeg = OUTPUT_ANGLE + diagnostics = messaging.new_message("starpilotCarControl") elif scenario == "invalid_flag": - output.actuatorsOutput.steeringLimitInfo.valid = False + diagnostics.starpilotCarControl.steeringLimitInfo.valid = False elif scenario == "zero_timestamp": - output.actuatorsOutput.steeringLimitInfo.monoTime = 0 + diagnostics.starpilotCarControl.steeringLimitInfo.monoTime = 0 elif scenario == "future_timestamp": - output.actuatorsOutput.steeringLimitInfo.monoTime = now_nanos + 1 + diagnostics.starpilotCarControl.steeringLimitInfo.monoTime = now_nanos + 1 elif scenario == "stale_timestamp": - output.actuatorsOutput.steeringLimitInfo.monoTime = now_nanos - MAX_DIAGNOSTIC_AGE_NANOS - 1 + diagnostics.starpilotCarControl.steeringLimitInfo.monoTime = now_nanos - MAX_DIAGNOSTIC_AGE_NANOS - 1 elif scenario == "unhealthy_output": output_healthy = False elif scenario == "unsupported_model": - cp, output = make_case(CAR.TESLA_MODEL_Y) + cp, output, diagnostics = make_case(CAR.TESLA_MODEL_Y) elif scenario == "cooperative_mode_disabled": - cp, output = make_case(cooperative_enabled=False) + cp, output, diagnostics = make_case(cooperative_enabled=False) - assert detect(cp, output, output_healthy=output_healthy, now_nanos=now_nanos) + assert detect(cp, output, diagnostics, output_healthy=output_healthy, now_nanos=now_nanos) @pytest.mark.parametrize(("requested_angle", "expected"), ( @@ -134,10 +139,10 @@ def test_unusable_diagnostics_keep_legacy_warning(scenario): (OUTPUT_ANGLE - NEXT_FLOAT32_AFTER_2_5, True), )) def test_fallback_preserves_legacy_strict_threshold(requested_angle, expected): - cp, output = make_case() - output.actuatorsOutput.steeringLimitInfo.valid = False + cp, output, diagnostics = make_case() + diagnostics.starpilotCarControl.steeringLimitInfo.valid = False - assert detect(cp, output, requested_angle=requested_angle) is expected + assert detect(cp, output, diagnostics, requested_angle=requested_angle) is expected @pytest.mark.parametrize("field", NUMERIC_FIELDS) @@ -147,15 +152,15 @@ def test_fallback_preserves_legacy_strict_threshold(requested_angle, expected): (-math.inf, OUTPUT_ANGLE, False), )) def test_nonfinite_diagnostic_values_use_legacy_fallback(field, bad_value, requested_angle, legacy_result): - cp, output = make_case() - setattr(output.actuatorsOutput.steeringLimitInfo, field, bad_value) + cp, output, diagnostics = make_case() + setattr(diagnostics.starpilotCarControl.steeringLimitInfo, field, bad_value) - assert detect(cp, output, requested_angle=requested_angle) is legacy_result + assert detect(cp, output, diagnostics, requested_angle=requested_angle) is legacy_result @pytest.mark.parametrize("field", ERROR_FIELDS) def test_negative_error_values_use_legacy_fallback(field): - cp, output = make_case() - setattr(output.actuatorsOutput.steeringLimitInfo, field, -0.1) + cp, output, diagnostics = make_case() + setattr(diagnostics.starpilotCarControl.steeringLimitInfo, field, -0.1) - assert detect(cp, output) + assert detect(cp, output, diagnostics) diff --git a/selfdrive/controls/tests/test_steering_saturation_publish.py b/selfdrive/controls/tests/test_steering_saturation_publish.py index 0c46e77947..0d7623483d 100644 --- a/selfdrive/controls/tests/test_steering_saturation_publish.py +++ b/selfdrive/controls/tests/test_steering_saturation_publish.py @@ -1,5 +1,6 @@ from types import SimpleNamespace +import cereal.messaging as messaging import pytest from cereal import car, custom, log @@ -27,7 +28,7 @@ class CapturePubMaster: class PublishSubMaster: - def __init__(self, car_output, selfdrive_time_nanos, output_healthy=True): + def __init__(self, car_output, starpilot_car_control, selfdrive_time_nanos, output_healthy=True): car_state = car.CarState.new_message() car_state.canValid = True @@ -43,6 +44,7 @@ class PublishSubMaster: "starpilotCarState": custom.StarPilotCarState.new_message().as_reader(), "selfdriveState": selfdrive_state.as_reader(), "carOutput": car_output.as_reader(), + "starpilotCarControl": starpilot_car_control.as_reader(), "driverAssistance": log.DriverAssistance.new_message().as_reader(), "driverMonitoringState": log.DriverMonitoringState.new_message().as_reader(), } @@ -69,19 +71,24 @@ def make_car_params(): def make_car_output(real_limit_error=0.0): output = car.CarOutput.new_message() output.actuatorsOutput.steeringAngleDeg = OUTPUT_ANGLE - info = output.actuatorsOutput.steeringLimitInfo + return output + + +def make_starpilot_car_control(real_limit_error=0.0): + message = messaging.new_message("starpilotCarControl", valid=True) + info = message.starpilotCarControl.steeringLimitInfo info.valid = True info.monoTime = SAMPLE_TIME_NANOS info.cooperativeOffsetDeg = 5.5 info.modelLimitErrorDeg = real_limit_error info.combinedLimitErrorDeg = real_limit_error - return output + return message -def make_controls(car_output, selfdrive_time_nanos, output_healthy=True): +def make_controls(car_output, starpilot_car_control, selfdrive_time_nanos, output_healthy=True): controls = Controls.__new__(Controls) controls.CP = make_car_params() - controls.sm = PublishSubMaster(car_output, selfdrive_time_nanos, output_healthy) + controls.sm = PublishSubMaster(car_output, starpilot_car_control, selfdrive_time_nanos, output_healthy) controls.pm = CapturePubMaster() controls.curvature = 0.0 controls.calibrated_pose = None @@ -100,7 +107,9 @@ def run_publish(monkeypatch, replay, selfdrive_time_nanos, host_time_nanos=HOST_ output_healthy=True, real_limit_error=0.0): monkeypatch.setattr(controlsd, "REPLAY", replay, raising=False) monkeypatch.setattr(controlsd.time, "monotonic_ns", lambda: host_time_nanos) - controls = make_controls(make_car_output(real_limit_error), selfdrive_time_nanos, output_healthy) + controls = make_controls( + make_car_output(real_limit_error), make_starpilot_car_control(real_limit_error), selfdrive_time_nanos, output_healthy, + ) cc = car.CarControl.new_message() cc.enabled = True cc.latActive = True diff --git a/selfdrive/selfdrived/tests/test_tesla_coop_saturation_warning.py b/selfdrive/selfdrived/tests/test_tesla_coop_saturation_warning.py index 24f9fb268d..60785f3240 100644 --- a/selfdrive/selfdrived/tests/test_tesla_coop_saturation_warning.py +++ b/selfdrive/selfdrived/tests/test_tesla_coop_saturation_warning.py @@ -53,6 +53,20 @@ def make_car_control(requested_angle, lat_active=True): return control +def make_starpilot_car_control(controller): + message = messaging.new_message("starpilotCarControl", valid=True) + values = controller.get_steering_limit_info() + info = message.starpilotCarControl.steeringLimitInfo + info.valid = values["valid"] + info.modelLimitErrorDeg = values["modelLimitErrorDeg"] + info.resumeLimitErrorDeg = values["resumeLimitErrorDeg"] + info.cooperativeLimitErrorDeg = values["cooperativeLimitErrorDeg"] + info.cooperativeOffsetDeg = values["cooperativeOffsetDeg"] + info.monoTime = values["monoTime"] + info.combinedLimitErrorDeg = values["combinedLimitErrorDeg"] + return message + + def run_controller_frame(controller, requested_angle, torque, speed, measured_angle, frame, lat_active=True, steering_disengage=False): now_nanos = START_NANOS + frame * 10_000_000 @@ -65,7 +79,8 @@ def run_controller_frame(controller, requested_angle, torque, speed, measured_an event = messaging.new_message("carOutput", valid=True) event.carOutput.actuatorsOutput = actuators restored = messaging.log_from_bytes(event.to_bytes()) - return restored.carOutput, now_nanos + diagnostics = messaging.log_from_bytes(make_starpilot_car_control(controller).to_bytes()) + return restored.carOutput, diagnostics, now_nanos def make_lateral_car_state(speed, measured_angle): @@ -163,13 +178,13 @@ def test_recorded_light_torque_reproduction_no_longer_reaches_warning(): output = None now_nanos = 0 for frame in range(260): - output, now_nanos = run_controller_frame( + output, diagnostics, now_nanos = run_controller_frame( tesla_controller, REPRO_REQUESTED_ANGLE, REPRO_TORQUE, REPRO_SPEED, REPRO_MEASURED_ANGLE, frame, ) if frame >= 210: legacy_limited = abs(REPRO_REQUESTED_ANGLE - output.actuatorsOutput.steeringAngleDeg) > 2.5 corrected_limited = is_angle_steering_limited( - CP.as_reader(), REPRO_REQUESTED_ANGLE, output, True, now_nanos, + CP.as_reader(), REPRO_REQUESTED_ANGLE, output, diagnostics, True, now_nanos, ) baseline_log = advance_angle_counter( baseline_counter, CP, lateral_state, legacy_limited, REPRO_DESIRED_CURVATURE, @@ -178,7 +193,7 @@ def test_recorded_light_torque_reproduction_no_longer_reaches_warning(): corrected_counter, CP, lateral_state, corrected_limited, REPRO_DESIRED_CURVATURE, ) - info = output.actuatorsOutput.steeringLimitInfo + info = diagnostics.starpilotCarControl.steeringLimitInfo errors = (info.modelLimitErrorDeg, info.resumeLimitErrorDeg, info.cooperativeLimitErrorDeg, info.combinedLimitErrorDeg) assert info.valid @@ -213,13 +228,13 @@ def test_persistent_real_model_limiting_with_light_torque_still_warns(): angle_log = None output = None for frame in range(60): - output, now_nanos = run_controller_frame( + output, diagnostics, now_nanos = run_controller_frame( tesla_controller, requested_angle, REPRO_TORQUE, speed, 0.0, frame, ) - limited = is_angle_steering_limited(CP.as_reader(), requested_angle, output, True, now_nanos) + limited = is_angle_steering_limited(CP.as_reader(), requested_angle, output, diagnostics, True, now_nanos) angle_log = advance_angle_counter(angle_counter, CP, lateral_state, limited, 0.01) - info = output.actuatorsOutput.steeringLimitInfo + info = diagnostics.starpilotCarControl.steeringLimitInfo assert info.valid assert info.modelLimitErrorDeg > 2.5 assert info.combinedLimitErrorDeg > 2.5 @@ -238,18 +253,18 @@ def test_default_old_output_uses_legacy_warning_path(): output_event = messaging.new_message("carOutput", valid=True) output_event.carOutput.actuatorsOutput.steeringAngleDeg = REPRO_MEASURED_ANGLE output = messaging.log_from_bytes(output_event.to_bytes()).carOutput + diagnostics = messaging.log_from_bytes(messaging.new_message("starpilotCarControl").to_bytes()) lateral_state = make_lateral_car_state(REPRO_SPEED, REPRO_MEASURED_ANGLE) angle_counter = LatControlAngle(CP.as_reader(), None, DT_CTRL) for frame in range(50): limited = is_angle_steering_limited( - CP.as_reader(), REPRO_REQUESTED_ANGLE, output, True, START_NANOS + frame * 10_000_000, + CP.as_reader(), REPRO_REQUESTED_ANGLE, output, diagnostics, True, START_NANOS + frame * 10_000_000, ) angle_log = advance_angle_counter( angle_counter, CP, lateral_state, limited, REPRO_DESIRED_CURVATURE, ) - assert not output.actuatorsOutput.steeringLimitInfo.valid assert angle_log.saturated selfdrived = configure_selfdrived(CP) events, _ = run_selfdrived_warning_path( @@ -265,15 +280,15 @@ def test_stale_real_offset_sample_uses_legacy_warning_path(): angle_counter = LatControlAngle(CP.as_reader(), None, DT_CTRL) for frame in range(260): - output, now_nanos = run_controller_frame( + output, diagnostics, now_nanos = run_controller_frame( tesla_controller, REPRO_REQUESTED_ANGLE, REPRO_TORQUE, REPRO_SPEED, REPRO_MEASURED_ANGLE, frame, ) - assert not is_angle_steering_limited(CP.as_reader(), REPRO_REQUESTED_ANGLE, output, True, now_nanos) - stale_now_nanos = output.actuatorsOutput.steeringLimitInfo.monoTime + 100_000_001 + assert not is_angle_steering_limited(CP.as_reader(), REPRO_REQUESTED_ANGLE, output, diagnostics, True, now_nanos) + stale_now_nanos = diagnostics.starpilotCarControl.steeringLimitInfo.monoTime + 100_000_001 for _ in range(50): limited = is_angle_steering_limited( - CP.as_reader(), REPRO_REQUESTED_ANGLE, output, True, stale_now_nanos, + CP.as_reader(), REPRO_REQUESTED_ANGLE, output, diagnostics, True, stale_now_nanos, ) angle_log = advance_angle_counter( angle_counter, CP, lateral_state, limited, REPRO_DESIRED_CURVATURE, @@ -291,24 +306,24 @@ def test_inactive_interval_clears_diagnostics_before_reengagement(): CP = make_params() tesla_controller = CarController(DBC[CAR.TESLA_MODEL_3], CP) - active, _ = run_controller_frame(tesla_controller, 0.0, REPRO_TORQUE, REPRO_SPEED, 0.0, 0) - inactive, _ = run_controller_frame( + active, active_diagnostics, _ = run_controller_frame(tesla_controller, 0.0, REPRO_TORQUE, REPRO_SPEED, 0.0, 0) + inactive, inactive_diagnostics, _ = run_controller_frame( tesla_controller, 0.0, REPRO_TORQUE, REPRO_SPEED, 0.0, 1, lat_active=False, ) - resumed, resumed_now = run_controller_frame( + resumed, resumed_diagnostics, resumed_now = run_controller_frame( tesla_controller, 0.0, REPRO_TORQUE, REPRO_SPEED, 0.0, 2, ) - overridden, _ = run_controller_frame( + overridden, overridden_diagnostics, _ = run_controller_frame( tesla_controller, 0.0, REPRO_TORQUE, REPRO_SPEED, 0.0, 3, steering_disengage=True, ) - assert active.actuatorsOutput.steeringLimitInfo.valid - assert not inactive.actuatorsOutput.steeringLimitInfo.valid - assert inactive.actuatorsOutput.steeringLimitInfo.monoTime == 0 - assert resumed.actuatorsOutput.steeringLimitInfo.valid - assert resumed.actuatorsOutput.steeringLimitInfo.monoTime == resumed_now - assert not overridden.actuatorsOutput.steeringLimitInfo.valid - assert overridden.actuatorsOutput.steeringLimitInfo.monoTime == 0 + assert active_diagnostics.starpilotCarControl.steeringLimitInfo.valid + assert not inactive_diagnostics.starpilotCarControl.steeringLimitInfo.valid + assert inactive_diagnostics.starpilotCarControl.steeringLimitInfo.monoTime == 0 + assert resumed_diagnostics.starpilotCarControl.steeringLimitInfo.valid + assert resumed_diagnostics.starpilotCarControl.steeringLimitInfo.monoTime == resumed_now + assert not overridden_diagnostics.starpilotCarControl.steeringLimitInfo.valid + assert overridden_diagnostics.starpilotCarControl.steeringLimitInfo.monoTime == 0 @pytest.mark.parametrize(("fault_field", "expected_event"), (