diff --git a/common/gps.py b/common/gps.py index 6f96d72e9..4d1a95c6a 100644 --- a/common/gps.py +++ b/common/gps.py @@ -2,7 +2,7 @@ from openpilot.common.params import Params def get_gps_location_service(params: Params) -> str: - if params.get_bool("UbloxAvailable"): + if params.get_bool("UbloxAvailable") or params.get_bool("CarGpsAvailable"): return "gpsLocationExternal" else: return "gpsLocation" diff --git a/common/libcommon.a b/common/libcommon.a index 9a8845935..4ac7bf8f7 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_keys.h b/common/params_keys.h index 62c24430d..2704a6c87 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -20,6 +20,7 @@ inline static std::unordered_map keys = { {"CameraDebugExpGain", {CLEAR_ON_MANAGER_START, STRING}}, {"CameraDebugExpTime", {CLEAR_ON_MANAGER_START, STRING}}, {"CarBatteryCapacity", {PERSISTENT, INT}}, + {"CarGpsAvailable", {CLEAR_ON_MANAGER_START, BOOL}}, {"CarParams", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BYTES}}, {"CarParamsCache", {CLEAR_ON_MANAGER_START, BYTES}}, {"CarParamsPersistent", {PERSISTENT, BYTES}}, diff --git a/common/params_pyx.so b/common/params_pyx.so index da57a57f9..7772b476c 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/opendbc_repo/opendbc/car/ford/carstate.py b/opendbc_repo/opendbc/car/ford/carstate.py index bb4cc0e48..a7dea6506 100644 --- a/opendbc_repo/opendbc/car/ford/carstate.py +++ b/opendbc_repo/opendbc/car/ford/carstate.py @@ -4,6 +4,7 @@ from opendbc.car import Bus, create_button_events, structs from opendbc.car.common.conversions import Conversions as CV from opendbc.car.ford.fordcan import CanBus from opendbc.car.ford.values import DBC, CarControllerParams, FordFlags +from opendbc.car.gps import get_car_gps_config from opendbc.car.interfaces import CarStateBase ButtonType = structs.CarState.ButtonEvent.Type @@ -27,12 +28,40 @@ class CarState(CarStateBase): self.lc_button = 0 self.lkas_available = False self.lateral_motion_control = None + self.lateral_control_status = None self.steering_angle_offset_deg = 0.0 + self.car_gps_config = get_car_gps_config(CP) + self.car_gps_supported = self.car_gps_config is not None + self.car_gps = None + self._car_gps_timestamp_nanos = 0 + + def _update_car_gps(self, cp) -> None: + if self.car_gps_config is None: + return + + timestamps = [max(cp.ts_nanos[name].values(), default=0) for name in self.car_gps_config.messages] + if not all(timestamps) or max(timestamps) - min(timestamps) > 2_000_000_000: + return + + timestamp_nanos = max(timestamps) + if timestamp_nanos <= self._car_gps_timestamp_nanos: + return + + gps = self.car_gps_config.decoder(*(cp.vl[name] for name in self.car_gps_config.messages)) + if gps is not None: + gps["timestamp_nanos"] = timestamp_nanos + self.car_gps = gps + self._car_gps_timestamp_nanos = timestamp_nanos + + def get_car_gps(self): + return self.car_gps def update(self, can_parsers, starpilot_toggles) -> structs.CarState: cp = can_parsers[Bus.pt] cp_cam = can_parsers[Bus.cam] + self._update_car_gps(cp) + ret = structs.CarState() if self.CP.flags & FordFlags.ALT_STEER_ANGLE: @@ -81,7 +110,8 @@ class CarState(CarStateBase): if self.CP.flags & FordFlags.CANFD: # this signal is always 0 on non-CAN FD cars - ret.steerFaultTemporary |= cp.vl["Lane_Assist_Data3_FD1"]["LatCtlSte_D_Stat"] not in (1, 2, 3) + self.lateral_control_status = int(cp.vl["Lane_Assist_Data3_FD1"]["LatCtlSte_D_Stat"]) + ret.steerFaultTemporary |= self.lateral_control_status not in (1, 2, 3) # cruise state is_metric = cp.vl["INSTRUMENT_PANEL"]["METRIC_UNITS"] == 1 if not self.CP.flags & FordFlags.CANFD else \ @@ -177,7 +207,9 @@ class CarState(CarStateBase): @staticmethod def get_can_parsers(CP): + gps_config = get_car_gps_config(CP) + gps_messages = [(name, 0) for name in gps_config.messages] if gps_config is not None else [] return { - Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus(CP).main), + Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], gps_messages, CanBus(CP).main), Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus(CP).camera), } diff --git a/opendbc_repo/opendbc/car/ford/tests/test_ford.py b/opendbc_repo/opendbc/car/ford/tests/test_ford.py index 3d3348f14..a93b94ee3 100644 --- a/opendbc_repo/opendbc/car/ford/tests/test_ford.py +++ b/opendbc_repo/opendbc/car/ford/tests/test_ford.py @@ -5,9 +5,10 @@ from types import SimpleNamespace from hypothesis import settings, given, strategies as st from parameterized import parameterized -from opendbc.car import gen_empty_fingerprint +from opendbc.car import Bus, gen_empty_fingerprint from opendbc.can import CANPacker from opendbc.car.ford import fordcan +from opendbc.car.gps import FORD_MACH_E_GPS_MESSAGES, get_car_gps_config, parse_ford_can_gps from opendbc.car.structs import CarParams from opendbc.car.fw_versions import build_fw_dict from opendbc.car.ford.interface import CarInterface @@ -177,6 +178,102 @@ def test_mach_e_longitudinal_toggle_controls_stock_acc_selection(): assert enhanced.safetyConfigs[-1].safetyParam & FordSafetyFlags.LONG_CONTROL +def test_mach_e_can_gps_decode(): + nav1 = { + "GpsHsphLattSth_D_Actl": 2, + "GpsHsphLongEast_D_Actl": 2, + "GPS_Latitude_Degrees": 37, + "GPS_Latitude_Minutes": 57, + "GPS_Latitude_Min_dec": 0.8864, + "GPS_Longitude_Degrees": -121, + "GPS_Longitude_Minutes": 44, + "GPS_Longitude_Min_dec": 0.22, + } + nav2 = { + "GpsUtcYr_No_Actl": 2026, + "GpsUtcMnth_No_Actl": 8, + "GpsUtcDay_No_Actl": 26, + "GPS_UTC_hours": 0, + "GPS_UTC_minutes": 24, + "GPS_UTC_seconds": 38, + "Gps_B_Falt": 0, + } + nav3 = { + "GPS_dimension": 2, + "GPS_Hdop": 0.6, + "GPS_Vdop": 0.8, + "GPS_Sat_num_in_view": 31, + "GPS_MSL_altitude": 90, + "GPS_Speed": 10, + "GPS_Heading": 180, + } + + gps = parse_ford_can_gps(nav1, nav2, nav3) + + assert gps is not None + assert gps["latitude"] == 37.96477333333333 + assert gps["longitude"] == -121.737 + assert abs(gps["altitude"] - 27.432) < 1e-9 + assert abs(gps["speed"] - 10 * 0.44704) < 1e-9 + assert gps["hasFix"] + assert gps["satelliteCount"] == 0 # 31 is Ford's invalid sentinel. + + +def test_mach_e_can_gps_fault_invalidates_fix(): + nav1 = { + "GpsHsphLattSth_D_Actl": 2, + "GpsHsphLongEast_D_Actl": 2, + "GPS_Latitude_Degrees": 37, + "GPS_Latitude_Minutes": 57, + "GPS_Latitude_Min_dec": 0.8864, + "GPS_Longitude_Degrees": -121, + "GPS_Longitude_Minutes": 44, + "GPS_Longitude_Min_dec": 0.22, + } + nav2 = { + "GpsUtcYr_No_Actl": 2026, + "GpsUtcMnth_No_Actl": 8, + "GpsUtcDay_No_Actl": 26, + "GPS_UTC_hours": 0, + "GPS_UTC_minutes": 24, + "GPS_UTC_seconds": 38, + "Gps_B_Falt": 1, + } + nav3 = { + "GPS_dimension": 2, + "GPS_Hdop": 0.6, + "GPS_Vdop": 0.8, + "GPS_Sat_num_in_view": 31, + "GPS_MSL_altitude": 90, + "GPS_Speed": 0, + "GPS_Heading": 180, + } + + gps = parse_ford_can_gps(nav1, nav2, nav3) + + assert gps is not None + assert not gps["hasFix"] + assert gps["latitude"] == 37.96477333333333 + assert gps["altitude"] == 0.0 + + +def test_mach_e_can_gps_messages_are_optional_main_bus_inputs(): + cp = CarInterface.get_params(CAR.FORD_MUSTANG_MACH_E_MK1, gen_empty_fingerprint(), [], False, False, False, None) + parser = CarInterface.CarState.get_can_parsers(cp)[Bus.pt] + gps_config = get_car_gps_config(cp) + + assert gps_config is not None + assert gps_config.messages == FORD_MACH_E_GPS_MESSAGES + assert gps_config.decoder is parse_ford_can_gps + assert set(parser.addresses) >= {0x462, 0x463, 0x464} + assert set(FORD_MACH_E_GPS_MESSAGES) == set(gps_config.messages) == { + parser.dbc.addr_to_msg[0x462].name, + parser.dbc.addr_to_msg[0x463].name, + parser.dbc.addr_to_msg[0x464].name, + } + assert all(parser.message_states[address].ignore_alive for address in (0x462, 0x463, 0x464)) + + def test_hands_free_cluster_status_is_opt_in(): packer = CANPacker("ford_lincoln_base_pt") CAN = SimpleNamespace(main=0) diff --git a/opendbc_repo/opendbc/car/gps.py b/opendbc_repo/opendbc/car/gps.py new file mode 100644 index 000000000..44f5fd155 --- /dev/null +++ b/opendbc_repo/opendbc/car/gps.py @@ -0,0 +1,115 @@ +import math + +from dataclasses import dataclass +from datetime import UTC, datetime +from collections.abc import Callable, Mapping +from typing import Any + +from opendbc.car.common.conversions import Conversions as CV +from opendbc.car.ford.values import CAR as FORD_CAR + + +CarGpsSample = dict[str, Any] +CanGpsDecoder = Callable[..., CarGpsSample | None] + + +@dataclass(frozen=True) +class CarGpsConfig: + """Vehicle-specific CAN GPS inputs and decoder.""" + + brand: str + messages: tuple[str, ...] + decoder: CanGpsDecoder + + +def _dop_accuracy(dop: float, default: float = 500.0) -> float: + """Convert Ford's dimensionless DOP into a conservative meter estimate.""" + return max(1.0, dop * 5.0) if 0.0 <= dop <= 5.8 else default + + +def parse_ford_can_gps(nav1: Mapping[str, float], nav2: Mapping[str, float], nav3: Mapping[str, float]) -> CarGpsSample | None: + """Decode the Ford APIM GPS messages into the fields used by gpsLocationExternal.""" + year = int(nav2["GpsUtcYr_No_Actl"]) + month = int(nav2["GpsUtcMnth_No_Actl"]) + day = int(nav2["GpsUtcDay_No_Actl"]) + hour = int(nav2["GPS_UTC_hours"]) + minute = int(nav2["GPS_UTC_minutes"]) + second = int(nav2["GPS_UTC_seconds"]) + try: + timestamp_ms = int(datetime(year, month, day, hour, minute, second, tzinfo=UTC).timestamp() * 1000) + except ValueError: + return None + + latitude_direction = int(nav1["GpsHsphLattSth_D_Actl"]) + longitude_direction = int(nav1["GpsHsphLongEast_D_Actl"]) + latitude_degrees = abs(float(nav1["GPS_Latitude_Degrees"])) + longitude_degrees = abs(float(nav1["GPS_Longitude_Degrees"])) + latitude_minutes = float(nav1["GPS_Latitude_Minutes"]) + float(nav1["GPS_Latitude_Min_dec"]) + longitude_minutes = float(nav1["GPS_Longitude_Minutes"]) + float(nav1["GPS_Longitude_Min_dec"]) + + coordinates_valid = ( + latitude_direction in (1, 2) and longitude_direction in (1, 2) and + 0.0 <= latitude_degrees <= 90.0 and 0.0 <= longitude_degrees <= 180.0 and + 0.0 <= latitude_minutes < 60.0 and 0.0 <= longitude_minutes < 60.0 + ) + latitude = (1.0 if latitude_direction == 2 else -1.0) * (latitude_degrees + latitude_minutes / 60.0) if coordinates_valid else 0.0 + longitude = (1.0 if longitude_direction == 1 else -1.0) * (longitude_degrees + longitude_minutes / 60.0) if coordinates_valid else 0.0 + + dimension = int(nav3["GPS_dimension"]) + has_fix = coordinates_valid and dimension in (1, 2) and int(nav2["Gps_B_Falt"]) == 0 + + speed_mph = float(nav3["GPS_Speed"]) + speed_mps = speed_mph * CV.MPH_TO_MS + heading = float(nav3["GPS_Heading"]) + if speed_mph in (254.0, 255.0) or not math.isfinite(speed_mps) or not 0.0 <= speed_mps <= 200.0: + speed_mps = 0.0 + if not math.isfinite(heading) or not 0.0 <= heading < 360.0: + heading = 0.0 + + vertical_accuracy = _dop_accuracy(float(nav3["GPS_Vdop"])) + horizontal_accuracy = _dop_accuracy(float(nav3["GPS_Hdop"])) + satellite_count = int(nav3["GPS_Sat_num_in_view"]) + if not 0 <= satellite_count < 30: # 30 and 31 are Ford's unknown/invalid values. + satellite_count = 0 + + heading_rad = math.radians(heading) + return { + "latitude": latitude, + "longitude": longitude, + "altitude": (float(nav3["GPS_MSL_altitude"]) * 0.3048) if has_fix else 0.0, + "speed": speed_mps, + "bearingDeg": heading, + "horizontalAccuracy": horizontal_accuracy, + "unixTimestampMillis": timestamp_ms, + "verticalAccuracy": vertical_accuracy, + "bearingAccuracyDeg": max(5.0, horizontal_accuracy * 2.0) if speed_mps > 1.0 else 180.0, + "speedAccuracy": max(0.5, horizontal_accuracy), + "hasFix": has_fix, + "satelliteCount": satellite_count, + "vNED": [speed_mps * math.cos(heading_rad), speed_mps * math.sin(heading_rad), 0.0], + } + + +FORD_MACH_E_GPS_MESSAGES = ( + "APIMGPS_Data_Nav_1_FD1", + "APIMGPS_Data_Nav_2_FD1", + "APIMGPS_Data_Nav_3_FD1", +) + + +CAR_GPS_CONFIGS: dict[str, CarGpsConfig] = { + FORD_CAR.FORD_MUSTANG_MACH_E_MK1: CarGpsConfig( + brand="ford", + messages=FORD_MACH_E_GPS_MESSAGES, + decoder=parse_ford_can_gps, + ), +} + + +def get_car_gps_config(CP) -> CarGpsConfig | None: + config = CAR_GPS_CONFIGS.get(CP.carFingerprint) + return config if config is not None and config.brand == CP.brand else None + + +def car_gps_available(CP) -> bool: + return get_car_gps_config(CP) is not None diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index c62c65161..6b309702e 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -730,7 +730,7 @@ class TestHyundaiFingerprint: @pytest.mark.parametrize("candidate, tracks_main_cruise", ( (CAR.HYUNDAI_ELANTRA_2021, False), - (CAR.HYUNDAI_ELANTRA_HEV_2024, True), + (CAR.HYUNDAI_ELANTRA_HEV_2024, False), (CAR.HYUNDAI_SONATA_HYBRID, True), )) def test_legacy_hyundai_long_main_cruise_tracking_is_vehicle_specific(self, candidate, tracks_main_cruise): diff --git a/opendbc_repo/opendbc/car/interfaces.py b/opendbc_repo/opendbc/car/interfaces.py index 46baf0172..ec833848f 100644 --- a/opendbc_repo/opendbc/car/interfaces.py +++ b/opendbc_repo/opendbc/car/interfaces.py @@ -245,10 +245,9 @@ class CarInterfaceBase(ABC): fp_ret.pcmCruiseSpeed = False CP.openpilotLongitudinalControl = True - # These classic Hyundai hybrids need their stock ACC main state tracked while - # using OP long. Their cluster/EPS state becomes inconsistent when AOL remains - # active after the physical ACC main state changes. - if candidate in (HYUNDAI.HYUNDAI_SONATA_HYBRID, HYUNDAI.HYUNDAI_ELANTRA_HEV_2024) and \ + # The Sonata Hybrid needs its stock ACC main state tracked while using OP long. + # The classic Elantra follows the default always-on ACC-main behavior instead. + if candidate == HYUNDAI.HYUNDAI_SONATA_HYBRID and \ CP.openpilotLongitudinalControl: fp_ret.flags |= HyundaiStarPilotFlags.MAIN_CRUISE_STATE_TRACKING.value diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 2fbe1fd14..55386a71e 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -85,9 +85,12 @@ 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', 'gpsLocationExternal']) self.can_rcv_cum_timeout_counter = 0 + self._last_car_gps_timestamp_nanos = 0 + self._last_car_gps_received_monotonic = 0.0 + self._last_car_gps_publish_monotonic = 0.0 self.CC_prev = car.CarControl.new_message() self.CS_prev = car.CarState.new_message() @@ -135,6 +138,8 @@ class Car: self.CI, self.CP, self.FPCP = CI, CI.CP, CI.FPCP self.RI = RI + self.params.put_bool("CarGpsAvailable", bool(getattr(self.CI.CS, 'car_gps_supported', False))) + interface_alternative_experience = self.CP.alternativeExperience self.CP.alternativeExperience = interface_alternative_experience openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle") @@ -332,6 +337,36 @@ class Car: def state_publish(self, CS: car.CarState, RD: structs.RadarDataT | None, FPCS: custom.StarPilotCarState): """carState and carParams publish loop""" + get_car_gps = getattr(self.CI.CS, 'get_car_gps', None) + car_gps = get_car_gps() if get_car_gps is not None else None + now = time.monotonic() + if car_gps is not None and car_gps['timestamp_nanos'] > self._last_car_gps_timestamp_nanos: + self._last_car_gps_timestamp_nanos = car_gps['timestamp_nanos'] + self._last_car_gps_received_monotonic = now + + if car_gps is not None and self._last_car_gps_received_monotonic > 0.0 and \ + now - self._last_car_gps_received_monotonic <= 2.5 and \ + now - self._last_car_gps_publish_monotonic >= 0.2: + gps_send = messaging.new_message('gpsLocationExternal', valid=True) + gps = gps_send.gpsLocationExternal + gps.flags = 0 + gps.latitude = car_gps['latitude'] + gps.longitude = car_gps['longitude'] + gps.altitude = car_gps['altitude'] + gps.speed = car_gps['speed'] + gps.bearingDeg = car_gps['bearingDeg'] + gps.horizontalAccuracy = car_gps['horizontalAccuracy'] + gps.unixTimestampMillis = car_gps['unixTimestampMillis'] + gps.source = log.GpsLocationData.SensorSource.car + gps.vNED = car_gps['vNED'] + gps.verticalAccuracy = car_gps['verticalAccuracy'] + gps.bearingAccuracyDeg = car_gps['bearingAccuracyDeg'] + gps.speedAccuracy = car_gps['speedAccuracy'] + gps.hasFix = car_gps['hasFix'] + gps.satelliteCount = car_gps['satelliteCount'] + self.pm.send('gpsLocationExternal', gps_send) + self._last_car_gps_publish_monotonic = now + # carParams - logged every 50 seconds (> 1 per segment) if self.sm.frame % int(50. / DT_CTRL) == 0: cp_send = messaging.new_message('carParams') diff --git a/selfdrive/ui/layouts/settings/starpilot/appearance.py b/selfdrive/ui/layouts/settings/starpilot/appearance.py index 5a9f1d200..528e9c490 100644 --- a/selfdrive/ui/layouts/settings/starpilot/appearance.py +++ b/selfdrive/ui/layouts/settings/starpilot/appearance.py @@ -348,8 +348,7 @@ class StarPilotAppearanceLayout(_SettingsPage): SettingRow("HideLeadMarker", "toggle", tr_noop("Hide Lead Marker"), subtitle="", get_state=lambda: self._params.get_bool("HideLeadMarker"), - set_state=lambda s: self._params.put_bool("HideLeadMarker", s), - visible=ol), + set_state=lambda s: self._params.put_bool("HideLeadMarker", s)), SettingRow("HideChangingLanesBanner", "toggle", tr_noop("Hide Changing Lanes Banner"), subtitle="", get_state=lambda: self._params.get_bool("HideChangingLanesBanner"), @@ -437,13 +436,12 @@ class StarPilotAppearanceLayout(_SettingsPage): SettingRow("ShowStoppingPoint", "toggle", tr_noop("Show Stop Sign"), subtitle="", get_state=lambda: self._params.get_bool("ShowStoppingPoint"), - set_state=lambda s: self._params.put_bool("ShowStoppingPoint", s), - enabled=ol), + set_state=lambda s: self._params.put_bool("ShowStoppingPoint", s)), SettingRow("ShowStoppingPointMetrics", "toggle", tr_noop("Stop Distance"), subtitle="", get_state=lambda: self._params.get_bool("ShowStoppingPointMetrics"), set_state=lambda s: self._params.put_bool("ShowStoppingPointMetrics", s), - enabled=lambda: self._params.get_bool("ShowStoppingPoint") and ol()), + enabled=lambda: self._params.get_bool("ShowStoppingPoint")), SettingRow("DeveloperMetrics", "toggle", tr_noop("Developer Metrics"), subtitle=tr_noop("Performance data, sensor readings, and system metrics."), get_state=lambda: self._params.get_bool("DeveloperMetrics"), diff --git a/selfdrive/ui/mici/onroad/model_renderer.py b/selfdrive/ui/mici/onroad/model_renderer.py index 45a07c4b7..17d9b6300 100644 --- a/selfdrive/ui/mici/onroad/model_renderer.py +++ b/selfdrive/ui/mici/onroad/model_renderer.py @@ -20,6 +20,7 @@ CLIP_MARGIN = 500 MIN_DRAW_DISTANCE = 10.0 MAX_DRAW_DISTANCE = 100.0 STOCK_LANE_LINES_COLOR = rl.Color(255, 255, 255, 255) +OCEAN_BLUE_LANE_LINES_COLOR = rl.Color(0, 176, 220, 255) DEFAULT_LANE_LINES_WIDTH = 4.0 DEFAULT_PATH_WIDTH = 6.1 DEFAULT_ROAD_EDGES_WIDTH = 2.0 @@ -360,7 +361,7 @@ class ModelRenderer(Widget): return LeadVehicle(glow=glow, chevron=chevron, fill_alpha=int(fill_alpha)) - def _lane_line_palette(self) -> tuple[bool, rl.Color, rl.Color]: + def _lane_line_palette(self) -> tuple[bool, rl.Color, rl.Color, bool]: stock_scheme = is_stock_color_scheme(self._params) line_status = UIStatus.ENGAGED if ui_state.status == UIStatus.DISENGAGED and ui_state.always_on_lateral_active else ui_state.status @@ -373,12 +374,18 @@ class ModelRenderer(Widget): if lane_color is None: lane_color = STOCK_LANE_LINES_COLOR if stock_scheme else get_theme_color("LaneLines", STOCK_LANE_LINES_COLOR) - return stock_scheme, edge_color, lane_color + lane_centering_active = bool(ui_state.starpilot_toggles.get("lane_centering", False)) and ( + ui_state.status == UIStatus.ENGAGED or ui_state.always_on_lateral_active + ) + return stock_scheme, edge_color, lane_color, lane_centering_active def _get_ll_color(self, prob: float, adjacent: bool, left: bool, stock_scheme: bool, - edge_color: rl.Color, lane_color: rl.Color): + edge_color: rl.Color, lane_color: rl.Color, lane_centering_active: bool = False): alpha = np.clip(prob, 0.0, 0.7) - if adjacent: + if lane_centering_active: + color = rl.Color(OCEAN_BLUE_LANE_LINES_COLOR.r, OCEAN_BLUE_LANE_LINES_COLOR.g, + OCEAN_BLUE_LANE_LINES_COLOR.b, int(alpha * OCEAN_BLUE_LANE_LINES_COLOR.a)) + elif adjacent: color = rl.Color(edge_color.r, edge_color.g, edge_color.b, int(alpha * edge_color.a)) # turn adjacent lls orange if torque is high @@ -401,13 +408,13 @@ class ModelRenderer(Widget): def _draw_lane_lines(self): """Draw lane lines and road edges""" """Two closest lines should be green (lane line or road edges)""" - stock_scheme, edge_color, lane_color = self._lane_line_palette() + stock_scheme, edge_color, lane_color, lane_centering_active = self._lane_line_palette() for i, lane_line in enumerate(self._lane_lines): if lane_line.projected_points.size == 0: continue color = self._get_ll_color(float(self._lane_line_probs[i]), i in (1, 2), i in (0, 1), - stock_scheme, edge_color, lane_color) + stock_scheme, edge_color, lane_color, lane_centering_active) draw_polygon(self._rect, lane_line.projected_points, color) for i, road_edge in enumerate(self._road_edges): diff --git a/selfdrive/ui/onroad/model_renderer.py b/selfdrive/ui/onroad/model_renderer.py index 189e9539a..ca263b27a 100644 --- a/selfdrive/ui/onroad/model_renderer.py +++ b/selfdrive/ui/onroad/model_renderer.py @@ -20,6 +20,7 @@ CLIP_MARGIN = 500 MIN_DRAW_DISTANCE = 10.0 MAX_DRAW_DISTANCE = 100.0 STOCK_LANE_LINES_COLOR = rl.Color(255, 255, 255, 255) +OCEAN_BLUE_LANE_LINES_COLOR = rl.Color(0, 176, 220, 255) DEFAULT_LANE_LINES_WIDTH = 4.0 DEFAULT_PATH_EDGE_WIDTH = 20.0 DEFAULT_PATH_WIDTH = 6.1 @@ -143,12 +144,7 @@ class ModelRenderer(Widget): self._is_metric = self._params.get_bool('IsMetric') if self._use_rainbow and sm.valid.get('carState', False): self._rainbow_path.update(max(sm['carState'].vEgo, 0.0)) - lead_info_enabled = self._lead_info_enabled - render_lead_indicator = ( - (self._longitudinal_control or lead_info_enabled) - and radar_state is not None - and lead_indicator_enabled(self._params) - ) + render_lead_indicator = self._should_render_lead_indicator(radar_state) # Update model data when needed model_updated = sm.updated['modelV2'] @@ -183,6 +179,9 @@ class ModelRenderer(Widget): self._draw_radar_tracks() + def _should_render_lead_indicator(self, radar_state) -> bool: + return radar_state is not None and lead_indicator_enabled(self._params) + def _update_raw_points(self, model): """Update raw 3D points from model data""" self._path.raw_points = np.array([model.position.x, model.position.y, model.position.z], dtype=np.float32).T @@ -372,8 +371,13 @@ class ModelRenderer(Widget): def _draw_lane_lines(self): """Draw lane lines and road edges""" + lane_centering_active = bool(ui_state.starpilot_toggles.get("lane_centering", False)) and ( + ui_state.status == UIStatus.ENGAGED or ui_state.always_on_lateral_active + ) lane_lines_override = get_param_color(self._params, "LaneLinesColor", STOCK_LANE_LINES_COLOR.a) - if lane_lines_override is not None: + if lane_centering_active: + lane_lines_color = OCEAN_BLUE_LANE_LINES_COLOR + elif lane_lines_override is not None: lane_lines_color = lane_lines_override elif is_stock_color_scheme(self._params): lane_lines_color = STOCK_LANE_LINES_COLOR diff --git a/selfdrive/ui/tests/test_adjacent_lane_renderer.py b/selfdrive/ui/tests/test_adjacent_lane_renderer.py index fd0fc8ffc..8bc14a1b9 100644 --- a/selfdrive/ui/tests/test_adjacent_lane_renderer.py +++ b/selfdrive/ui/tests/test_adjacent_lane_renderer.py @@ -16,6 +16,35 @@ class _FakeSubMaster: return self._plan +class _FakeParams: + def __init__(self, *, hide_lead_marker=False): + self.hide_lead_marker = hide_lead_marker + + def get(self, key): + return b"1" if key == "HideLeadMarker" else None + + def get_bool(self, key): + assert key == "HideLeadMarker" + return self.hide_lead_marker + + +def test_lead_indicator_renders_without_longitudinal_control(): + renderer = object.__new__(model_renderer.ModelRenderer) + renderer._params = _FakeParams() + renderer._longitudinal_control = False + renderer._lead_info_enabled = False + + assert renderer._should_render_lead_indicator(SimpleNamespace()) + + +def test_lead_indicator_still_honors_visibility_setting(): + renderer = object.__new__(model_renderer.ModelRenderer) + renderer._params = _FakeParams(hide_lead_marker=True) + + assert not renderer._should_render_lead_indicator(SimpleNamespace()) + assert not renderer._should_render_lead_indicator(None) + + @pytest.mark.parametrize( ("lane_width_left", "lane_width_right", "expected_side"), [ diff --git a/starpilot/car/ford/lateral.py b/starpilot/car/ford/lateral.py index cbd3da231..2a4c35239 100644 --- a/starpilot/car/ford/lateral.py +++ b/starpilot/car/ford/lateral.py @@ -30,9 +30,11 @@ STEER_DT = CarControllerParams.STEER_STEP * DT_CTRL CURVATURE_LOOKAHEAD_MIN = 0.20 CURVATURE_LOOKAHEAD_MAX = 0.40 ANGLE_HANDOFF_PRESS_SECONDS = 0.5 +HANDOFF_PAUSE_MIN_FRAMES = 3 HANDOFF_PAUSE_FRAMES = 6 HANDOFF_COOLDOWN_SECONDS = 2.0 HANDOFF_MAX_PATH_ANGLE = 0.10 +LAT_CTL_STATUS_AVAILABLE = 1 STALL_GAP_MIN = 2.0 * CarControllerParams.CURVATURE_ERROR STALL_HOLD_SECONDS = 0.5 STALL_MAX_RECOVERIES = 3 @@ -239,6 +241,16 @@ class FordLateralController: self.handoff_press_timer = 0.0 if self.angle_pause_frames > 0: + pause_frames_sent = HANDOFF_PAUSE_FRAMES - self.angle_pause_frames + pscm_available = getattr(CS, "lateral_control_status", None) == LAT_CTL_STATUS_AVAILABLE + # CAN-FD reports when the mode-0 reset has reached the PSCM. Keep a short minimum + # dwell, then resume immediately on that acknowledgement; retain the full pulse + # as a fallback for platforms without the status signal. + if pause_frames_sent >= HANDOFF_PAUSE_MIN_FRAMES and pscm_available: + self.angle_pause_frames = 0 + self.angle_pause_cooldown = HANDOFF_COOLDOWN_SECONDS + return False + self.angle_pause_frames -= 1 if self.angle_pause_frames == 0: self.angle_pause_cooldown = HANDOFF_COOLDOWN_SECONDS diff --git a/starpilot/car/ford/tests/test_lateral.py b/starpilot/car/ford/tests/test_lateral.py index c160c8164..8748ca128 100644 --- a/starpilot/car/ford/tests/test_lateral.py +++ b/starpilot/car/ford/tests/test_lateral.py @@ -3,7 +3,7 @@ from types import SimpleNamespace import pytest -from ..lateral import HANDOFF_PAUSE_FRAMES, FordLateralController, HumanTurnDetector +from ..lateral import HANDOFF_PAUSE_FRAMES, HANDOFF_PAUSE_MIN_FRAMES, FordLateralController, HumanTurnDetector class FakeSubMaster(dict): @@ -25,13 +25,16 @@ def controller(monkeypatch): return controller -def car_state(speed=15.0, curvature=0.0, steering_pressed=False, steering_angle=0.0): - return SimpleNamespace(out=SimpleNamespace( +def car_state(speed=15.0, curvature=0.0, steering_pressed=False, steering_angle=0.0, lateral_control_status=None): + state = SimpleNamespace(out=SimpleNamespace( vEgoRaw=speed, yawRate=-curvature * speed, steeringPressed=steering_pressed, steeringAngleDeg=steering_angle, )) + if lateral_control_status is not None: + state.lateral_control_status = lateral_control_status + return state def test_human_turn_requires_sustained_input(): @@ -156,6 +159,23 @@ def test_short_driver_correction_does_not_pause_angle_control(controller): assert controller.update_angle(CC, car_state(), actuators).active +def test_angle_control_resumes_after_pscm_acknowledges_pause(controller): + controller.human_turn_enabled = True + CC = SimpleNamespace(latActive=True) + actuators = SimpleNamespace(curvature=0.001) + + for _ in range(10): + assert controller.update_angle( + CC, car_state(steering_pressed=True, steering_angle=10.0), actuators).active + + for _ in range(HANDOFF_PAUSE_MIN_FRAMES): + assert not controller.update_angle( + CC, car_state(lateral_control_status=1), actuators).active + + assert controller.update_angle( + CC, car_state(lateral_control_status=1), actuators).active + + def test_angle_control_recovers_from_bounded_tracking_stall(controller): controller.human_turn_enabled = True controller.angle_blend = 0.0 diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 201dc518b..41be05894 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -707,7 +707,7 @@ class StarPilotVariables: toggle.hide_distance_profile_banner = self.get_value("HideDistanceProfileBanner", condition=advanced_custom_ui and not toggle.debug_mode) toggle.hide_turning_banner = self.get_value("HideTurningBanner", condition=advanced_custom_ui and not toggle.debug_mode) toggle.hide_dm_icon = self.get_value("HideDMIcon", condition=advanced_custom_ui) and not toggle.debug_mode - toggle.hide_lead_marker = self.get_value("HideLeadMarker", condition=advanced_custom_ui and toggle.openpilot_longitudinal and not toggle.debug_mode) + toggle.hide_lead_marker = self.get_value("HideLeadMarker", condition=advanced_custom_ui and not toggle.debug_mode) toggle.hide_max_speed = self.get_value("HideMaxSpeed", condition=advanced_custom_ui and not toggle.debug_mode) toggle.hide_speed = self.get_value("HideSpeed", condition=advanced_custom_ui and not toggle.debug_mode) toggle.hide_speed_limit = self.get_value("HideSpeedLimit", condition=advanced_custom_ui and not toggle.debug_mode) @@ -956,7 +956,7 @@ class StarPilotVariables: developer_widgets = self.get_value("DeveloperWidgets", condition=toggle.developer_ui) toggle.adjacent_lead_tracking = has_radar and (self.get_value("AdjacentLeadsUI", condition=developer_widgets) or toggle.debug_mode) toggle.radar_tracks = has_radar and (self.get_value("RadarTracksUI", condition=developer_widgets) or toggle.debug_mode) - toggle.show_stopping_point = toggle.openpilot_longitudinal and (self.get_value("ShowStoppingPoint", condition=developer_widgets) or toggle.debug_mode) + toggle.show_stopping_point = self.get_value("ShowStoppingPoint", condition=developer_widgets) or toggle.debug_mode toggle.show_stopping_point_metrics = self.get_value("ShowStoppingPointMetrics", condition=toggle.show_stopping_point) or toggle.debug_mode device_management = self.get_value("DeviceManagement") diff --git a/starpilot/system/the_galaxy/assets/components/tools/tuning.js b/starpilot/system/the_galaxy/assets/components/tools/tuning.js index 88ad20660..54a7423a4 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/tuning.js +++ b/starpilot/system/the_galaxy/assets/components/tools/tuning.js @@ -276,6 +276,7 @@ async function fetchStatus() { state.status = { ...(payload.status || {}), isOnroad: !!payload.isOnroad, + laneCentering: !!payload.laneCentering, } if (payload.activeTrial !== undefined) { state.workspace = { @@ -1125,11 +1126,14 @@ export function Tuning() {

Analyze one or more local routes, review deterministic lateral findings, apply a bounded trial, drive, then revert or refine.

+

+ Before using FLM: turn Lane Centering off. FLM must analyze the model's unmodified lateral request; routes recorded with Lane Centering enabled are excluded. +

@@ -1192,6 +1196,10 @@ export function Tuning() {

FLM analysis is offroad-only. Stop the car and go offroad before starting a run.

` : ""} + ${() => state.status?.laneCentering ? html` +

FLM is blocked while Lane Centering is enabled. Turn it off before starting analysis.

+ ` : ""} + ${() => state.workspace?.activeTrial?.rollbackAvailable === false ? html`

The original rollback data is unavailable. Keep the current tune as the new baseline before applying another trial. @@ -1366,6 +1374,7 @@ export function Tuning() {

Processed Segments: ${safeCount(state.report.summary?.processedSegments)}

Skipped Segments: ${safeCount(state.report.summary?.skippedSegments)}

Driver-Override Samples Excluded: ${safeCount(state.report.summary?.excludedDriverOverrideSamples)}

+

Lane Centering Segments Excluded: ${safeCount(state.report.summary?.laneCenteringExcludedSegments)}

qlog Fallback: ${state.report.summary?.usedQlogFallback ? "Yes" : "No"}

Samples: ${safeCount(state.report.summary?.sampleCount)}

diff --git a/starpilot/system/the_galaxy/flm_workspace.py b/starpilot/system/the_galaxy/flm_workspace.py index 2ae2b2ad0..9af8a148d 100644 --- a/starpilot/system/the_galaxy/flm_workspace.py +++ b/starpilot/system/the_galaxy/flm_workspace.py @@ -385,6 +385,12 @@ def _require_flm_offroad(params: Params | None = None) -> None: raise FLMAnalysisCancelled("FLM analysis stopped because the vehicle went onroad.") +def _require_flm_lane_centering_off(params: Params | None = None) -> None: + params = params or Params(return_defaults=True) + if params.get_bool("LaneCentering"): + raise FLMAnalysisCancelled("FLM analysis requires Lane Centering to be off.") + + def flm_analyzer_running() -> bool: process = FLM_ANALYZER_PROCESS if process is not None and process.poll() is None: @@ -557,6 +563,7 @@ def start_flm_background_analysis(route_names: list[str], footage_paths: list[st segment_ranges = normalize_segment_ranges(route_names, segment_ranges) try: _require_flm_offroad() + _require_flm_lane_centering_off() except FLMAnalysisCancelled: return False @@ -879,6 +886,10 @@ def _decode_init_param(init, key: str) -> str: return str(value or "") +def _init_param_enabled(init_data: dict[str, str], key: str) -> bool: + return init_data.get(key, "").strip().lower() in ("1", "true", "yes", "on") + + def _car_params_control_path(car_params) -> str: angle_type = getattr(car.CarParams.SteerControlType, "angle", None) if angle_type is not None and getattr(car_params, "steerControlType", None) == angle_type: @@ -947,6 +958,7 @@ def _segment_samples(segment_source: RouteSource, params: Params | None = None) "gitCommit": str(getattr(init, "gitCommit", "") or ""), "gitBranch": str(getattr(init, "gitBranch", "") or ""), "ForceTorqueController": _decode_init_param(init, "ForceTorqueController"), + "LaneCentering": _decode_init_param(init, "LaneCentering"), } continue if which == "carState": @@ -2369,6 +2381,8 @@ def _render_report_html(report: dict[str, Any]) -> str: start_here_lines = "".join(f"
  • {line}
    • " for line in report.get("addTheseParametersAndStartHere", [])) start_here_html = f"

    Add These Parameters And Start Here

    " if start_here_lines else "" + warnings_html = "".join(f"
  • {warning}
    • " for warning in report.get("warnings", [])) + warnings_block = f"

    Warnings

    " if warnings_html else "" findings_block = "".join(findings_html) or "

    No strong findings.

    " profiles_block = "".join(profile_html) or "

    No trial profiles generated.

    " return ( @@ -2388,6 +2402,7 @@ def _render_report_html(report: dict[str, Any]) -> str: f"

    Control Path

    {report['car'].get('controlPath', 'unknown')}

    " f"

    Friction Family

    {report['capabilities'].get('frictionFamily', 'standard')}

    " f"

    Nonlinear Torque Map

    {'Asymmetric left/right siglin' if report['capabilities'].get('nonlinearTorqueMap', {}).get('asymmetric') else ('Symmetric siglin' if report['capabilities'].get('nonlinearTorqueMap') else 'Not detected')}

    " + f"{warnings_block}" f"{''.join(path_html)}" f"{start_here_html}" f"

    Active Findings: {primary_path.get('title', 'Recommendations')}

    " @@ -2403,6 +2418,7 @@ def analyze_routes(route_names: list[str], footage_paths: list[str], feedback: d ensure_flm_workspace() params = Params(return_defaults=True) _require_flm_offroad(params) + _require_flm_lane_centering_off(params) report_id = report_id or f"flm-{int(time.time())}" feedback = feedback or {} segment_ranges = normalize_segment_ranges(route_names, segment_ranges) @@ -2420,8 +2436,11 @@ def analyze_routes(route_names: list[str], footage_paths: list[str], feedback: d skipped_segments = 0 last_skipped_segment = "" last_skip_reason = "" + lane_centering_excluded_segments = 0 + lane_centering_excluded_routes: dict[str, int] = {} for idx, source in enumerate(sources, start=1): _require_flm_offroad(params) + _require_flm_lane_centering_off(params) _write_flm_status({ "pid": os.getpid(), "startedAt": time.time(), @@ -2469,6 +2488,14 @@ def analyze_routes(route_names: list[str], footage_paths: list[str], feedback: d skipped_segments += 1 continue _require_flm_offroad(params) + _require_flm_lane_centering_off(params) + if _init_param_enabled(segment_init, "LaneCentering"): + lane_centering_excluded_segments += 1 + lane_centering_excluded_routes[source.route] = lane_centering_excluded_routes.get(source.route, 0) + 1 + skipped_segments += 1 + last_skipped_segment = source.segment + last_skip_reason = "Lane Centering was enabled in the recorded route." + continue if segment_car_params is not None: car_params_candidates.append(segment_car_params) if segment_init and not init_data: @@ -2480,7 +2507,19 @@ def analyze_routes(route_names: list[str], footage_paths: list[str], feedback: d all_samples.extend(segment_samples) processed_segments += 1 + for route, count in sorted(lane_centering_excluded_routes.items()): + warnings.append( + f"{route}: excluded {count} segment(s) because Lane Centering was enabled when they were recorded. " + "Turn it off before recording routes for FLM." + ) + if not car_params_candidates: + if lane_centering_excluded_segments: + excluded_routes = ", ".join(sorted(lane_centering_excluded_routes)) + raise RuntimeError( + "All selected FLM segments were recorded with Lane Centering enabled and were excluded. " + f"Turn Lane Centering off, record a fresh route, and try again ({excluded_routes})." + ) raise RuntimeError("No carParams were found in the selected routes.") _require_flm_offroad(params) @@ -2619,6 +2658,9 @@ def analyze_routes(route_names: list[str], footage_paths: list[str], feedback: d "processedSegments": processed_segments, "skippedSegments": skipped_segments, "usedQlogFallback": used_qlog, + "laneCenteringExcludedSegments": lane_centering_excluded_segments, + "laneCenteringExcludedRoutes": lane_centering_excluded_routes, + "laneCenteringRequiredOff": True, }, "primaryPathKey": path_decision["primaryPathKey"], "selectedPathKey": path_decision["primaryPathKey"], diff --git a/starpilot/system/the_galaxy/tests/test_flm_workspace.py b/starpilot/system/the_galaxy/tests/test_flm_workspace.py index 580e7fdcd..46d313b4a 100644 --- a/starpilot/system/the_galaxy/tests/test_flm_workspace.py +++ b/starpilot/system/the_galaxy/tests/test_flm_workspace.py @@ -272,6 +272,24 @@ def test_analysis_is_rejected_while_onroad(tmp_path): assert module.start_flm_background_analysis(["route"], [str(tmp_path)]) is False +def test_analysis_requires_lane_centering_off(tmp_path): + module, fake_params_cls = _load_flm_workspace_module(tmp_path) + fake_params_cls._store = {"LaneCentering": True} + + with pytest.raises(module.FLMAnalysisCancelled, match="Lane Centering"): + module._require_flm_lane_centering_off() + assert module.start_flm_background_analysis(["route"], [str(tmp_path)]) is False + + +def test_init_param_enabled_accepts_boolean_values(tmp_path): + module, _ = _load_flm_workspace_module(tmp_path) + + assert module._init_param_enabled({"LaneCentering": "1"}, "LaneCentering") + assert module._init_param_enabled({"LaneCentering": "true"}, "LaneCentering") + assert not module._init_param_enabled({"LaneCentering": "0"}, "LaneCentering") + assert not module._init_param_enabled({}, "LaneCentering") + + def test_segment_analysis_stops_on_mid_run_onroad_transition(tmp_path, monkeypatch): module, _ = _load_flm_workspace_module(tmp_path) diff --git a/starpilot/system/the_galaxy/the_galaxy.py b/starpilot/system/the_galaxy/the_galaxy.py index d46005ad8..5891ada7c 100644 --- a/starpilot/system/the_galaxy/the_galaxy.py +++ b/starpilot/system/the_galaxy/the_galaxy.py @@ -6728,11 +6728,13 @@ def setup(app): @app.route("/api/flm/status", methods=["GET"]) def get_flm_status(): is_onroad = params.get_bool("IsOnroad") + lane_centering = params.get_bool("LaneCentering") if is_onroad: flm_workspace.cancel_flm_if_onroad() workspace = flm_workspace.list_workspace() return jsonify({ "isOnroad": is_onroad, + "laneCentering": lane_centering, "status": flm_workspace.read_flm_status(), "activeTrial": workspace.get("activeTrial"), "reports": workspace.get("reports", [])[:10], @@ -6744,6 +6746,10 @@ def setup(app): def start_flm_analysis(): if params.get_bool("IsOnroad"): return jsonify({"error": "FLM analysis can only run offroad."}), 409 + if params.get_bool("LaneCentering"): + return jsonify({ + "error": "Turn Lane Centering off before running FLM. Its correction must not be mixed into lateral-tuning analysis." + }), 409 data = request.get_json(silent=True) or {} route_names = [str(route).strip() for route in data.get("routes", []) if str(route).strip()] diff --git a/system/manager/process_config.py b/system/manager/process_config.py index ce878a925..4ea1879d3 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -7,6 +7,7 @@ from types import SimpleNamespace from cereal import car from openpilot.common.params import Params +from opendbc.car.gps import car_gps_available from openpilot.system.hardware import HARDWARE, PC, TICI from openpilot.system.manager.process import PythonProcess, NativeProcess, DaemonProcess @@ -30,11 +31,21 @@ def logging(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: def ublox_available() -> bool: return os.path.exists('/dev/ttyHS0') and not os.path.exists('/persist/comma/use-quectel-gps') + +def update_car_gps_param(params: Params, CP: car.CarParams) -> bool: + available = car_gps_available(CP) + if available != params.get_bool("CarGpsAvailable"): + params.put_bool("CarGpsAvailable", available) + return available + def ublox(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool: + car_gps = update_car_gps_param(params, CP) use_ublox = ublox_available() if use_ublox != params.get_bool("UbloxAvailable"): params.put_bool("UbloxAvailable", use_ublox) - return started and use_ublox + # The Mach-E's CAN GPS is the preferred external source for now. Do not let + # the no-fix comma GNSS publisher race it on gpsLocationExternal. + return started and use_ublox and not car_gps def joystick(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool: return started and params.get_bool("JoystickDebugMode") @@ -52,6 +63,7 @@ def not_long_maneuver(started: bool, params: Params, CP: car.CarParams, starpilo return started and not params.get_bool("LongitudinalManeuverMode") def qcomgps(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool: + update_car_gps_param(params, CP) return started and not ublox_available() def always_run(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool: