diff --git a/.vscode/settings.json b/.vscode/settings.json index daf74ca777..811306f399 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -12,5 +12,15 @@ "**/.git": true, "**/.venv": true, "**/__pycache__": true - } -} \ No newline at end of file + }, + "python.analysis.exclude": [ + "**/.git", + "**/.venv", + "**/__pycache__", + // exclude directories that should be using the symlinked version + "common/**", + "selfdrive/**", + "system/**", + "tools/**", + ] +} diff --git a/pyproject.toml b/pyproject.toml index 8894d5eca2..10b99a9ddc 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -177,6 +177,8 @@ lint.ignore = ["E741", "E402", "C408", "ISC003", "B027", "B024"] line-length = 160 target-version="py311" exclude = [ + "body", + "cereal", "panda", "opendbc", "rednose_repo", diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 9231794a6c..82335500d8 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -70,7 +70,7 @@ class CarD: if prev_cp is not None: self.params.put("CarParamsPrevRoute", prev_cp) - # Write CarParams for radard + # Write CarParams for controls and radard cp_bytes = self.CP.to_bytes() self.params.put("CarParams", cp_bytes) self.params.put_nonblocking("CarParamsCache", cp_bytes) diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 199e0bba4d..e79da0f473 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -710,7 +710,8 @@ PLATFORM_CODE_ECUS = [Ecu.fwdRadar, Ecu.fwdCamera, Ecu.eps] # TODO: there are date codes in the ABS firmware versions in hex DATE_FW_ECUS = [Ecu.fwdCamera] -ALL_HYUNDAI_ECUS = [Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.engine, Ecu.parkingAdas, Ecu.transmission, Ecu.adas, Ecu.hvac, Ecu.cornerRadar] +ALL_HYUNDAI_ECUS = [Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.engine, Ecu.parkingAdas, + Ecu.transmission, Ecu.adas, Ecu.hvac, Ecu.cornerRadar, Ecu.combinationMeter] FW_QUERY_CONFIG = FwQueryConfig( requests=[ @@ -810,10 +811,11 @@ FW_QUERY_CONFIG = FwQueryConfig( Ecu.abs: [CAR.PALISADE, CAR.SONATA], }, extra_ecus=[ - (Ecu.adas, 0x730, None), # ADAS Driving ECU on HDA2 platforms - (Ecu.parkingAdas, 0x7b1, None), # ADAS Parking ECU (may exist on all platforms) - (Ecu.hvac, 0x7b3, None), # HVAC Control Assembly + (Ecu.adas, 0x730, None), # ADAS Driving ECU on HDA2 platforms + (Ecu.parkingAdas, 0x7b1, None), # ADAS Parking ECU (may exist on all platforms) + (Ecu.hvac, 0x7b3, None), # HVAC Control Assembly (Ecu.cornerRadar, 0x7b7, None), + (Ecu.combinationMeter, 0x7c6, None), # CAN FD Instrument cluster ], # Custom fuzzy fingerprinting function using platform codes, part numbers + FW dates: match_fw_to_car_fuzzy=match_fw_to_car_fuzzy, diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index a94b97de20..60cc3a0090 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -30,11 +30,6 @@ class CarInterface(CarInterfaceBase): def _update(self, c): ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam) - buttonEvents = [] - be = car.CarState.ButtonEvent.new_message() - be.type = car.CarState.ButtonEvent.Type.accelCruise - buttonEvents.append(be) - events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.brake]) if self.CS.lkas_enabled: diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index ebf0ca9062..821ff2c151 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -29,6 +29,16 @@ class CarState(CarStateBase): cp_brakes = cp_body if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp ret.brakePressed = cp_brakes.vl["Brake_Status"]["Brake"] == 1 + cp_es_distance = cp_body if self.CP.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.HYBRID) else cp_cam + if not (self.CP.flags & SubaruFlags.HYBRID): + eyesight_fault = bool(cp_es_distance.vl["ES_Distance"]["Cruise_Fault"]) + + # if openpilot is controlling long, an eyesight fault is a non-critical fault. otherwise it's an ACC fault + if self.CP.openpilotLongitudinalControl: + ret.carFaultedNonCritical = eyesight_fault + else: + ret.accFaulted = eyesight_fault + cp_wheels = cp_body if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp ret.wheelSpeeds = self.get_wheel_speeds( cp_wheels.vl["Wheel_Speeds"]["FL"], @@ -84,7 +94,6 @@ class CarState(CarStateBase): cp.vl["BodyInfo"]["DOOR_OPEN_FL"]]) ret.steerFaultPermanent = cp.vl["Steering_Torque"]["Steer_Error_1"] == 1 - cp_es_distance = cp_body if self.CP.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.HYBRID) else cp_cam if self.CP.flags & SubaruFlags.PREGLOBAL: self.cruise_button = cp_cam.vl["ES_Distance"]["Cruise_Button"] self.ready = not cp_cam.vl["ES_DashStatus"]["Not_Ready_Startup"] diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index cc963404b7..5e135ca658 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -234,21 +234,24 @@ FW_QUERY_CONFIG = FwQueryConfig( [StdQueries.TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST], [StdQueries.TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE], whitelist_ecus=[Ecu.abs, Ecu.eps, Ecu.fwdCamera, Ecu.engine, Ecu.transmission], + logging=True, ), + # Non-OBD requests # Some Eyesight modules fail on TESTER_PRESENT_REQUEST # TODO: check if this resolves the fingerprinting issue for the 2023 Ascent and other new Subaru cars Request( [SUBARU_VERSION_REQUEST], [SUBARU_VERSION_RESPONSE], whitelist_ecus=[Ecu.fwdCamera], + bus=0, ), - # Non-OBD requests Request( [StdQueries.TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST], [StdQueries.TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE], whitelist_ecus=[Ecu.abs, Ecu.eps, Ecu.fwdCamera, Ecu.engine, Ecu.transmission], bus=0, ), + # GEN2 powertrain bus query Request( [StdQueries.TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST], [StdQueries.TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE], diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index 92ee7fa923..265f052b16 100755 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -10,6 +10,7 @@ from openpilot.selfdrive.car.nissan.values import CAR as NISSAN from openpilot.selfdrive.car.mazda.values import CAR as MAZDA from openpilot.selfdrive.car.subaru.values import CAR as SUBARU from openpilot.selfdrive.car.toyota.values import CAR as TOYOTA +from openpilot.selfdrive.car.values import Platform from openpilot.selfdrive.car.volkswagen.values import CAR as VOLKSWAGEN from openpilot.selfdrive.car.tesla.values import CAR as TESLA from openpilot.selfdrive.car.body.values import CAR as COMMA @@ -29,7 +30,7 @@ non_tested_cars = [ class CarTestRoute(NamedTuple): route: str - car_model: str | None + car_model: Platform | None segment: int | None = None diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index b7d20e5a83..1ef8c5b676 100755 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -19,6 +19,7 @@ from openpilot.selfdrive.car.fingerprints import all_known_cars from openpilot.selfdrive.car.car_helpers import FRAME_FINGERPRINT, interfaces from openpilot.selfdrive.car.honda.values import CAR as HONDA, HondaFlags from openpilot.selfdrive.car.tests.routes import non_tested_cars, routes, CarTestRoute +from openpilot.selfdrive.car.values import PLATFORMS, Platform from openpilot.selfdrive.controls.controlsd import Controls from openpilot.selfdrive.test.helpers import read_segment_list from openpilot.system.hardware.hw import DEFAULT_DOWNLOAD_CACHE_ROOT @@ -64,7 +65,7 @@ def get_test_cases() -> list[tuple[str, CarTestRoute | None]]: @pytest.mark.slow @pytest.mark.shared_download_cache class TestCarModelBase(unittest.TestCase): - car_model: str | None = None + platform: Platform | None = None test_route: CarTestRoute | None = None test_route_on_bucket: bool = True # whether the route is on the preserved CI bucket @@ -93,8 +94,8 @@ class TestCarModelBase(unittest.TestCase): car_fw = msg.carParams.carFw if msg.carParams.openpilotLongitudinalControl: experimental_long = True - if cls.car_model is None and not cls.ci: - cls.car_model = msg.carParams.carFingerprint + if cls.platform is None and not cls.ci: + cls.platform = PLATFORMS.get(msg.carParams.carFingerprint) # Log which can frame the panda safety mode left ELM327, for CAN validity checks elif msg.which() == 'pandaStates': @@ -155,15 +156,11 @@ class TestCarModelBase(unittest.TestCase): if cls.__name__ == 'TestCarModel' or cls.__name__.endswith('Base'): raise unittest.SkipTest - if 'FILTER' in os.environ: - if not cls.car_model.startswith(tuple(os.environ.get('FILTER').split(','))): - raise unittest.SkipTest - if cls.test_route is None: - if cls.car_model in non_tested_cars: - print(f"Skipping tests for {cls.car_model}: missing route") + if cls.platform in non_tested_cars: + print(f"Skipping tests for {cls.platform}: missing route") raise unittest.SkipTest - raise Exception(f"missing test route for {cls.car_model}") + raise Exception(f"missing test route for {cls.platform}") car_fw, can_msgs, experimental_long = cls.get_testing_data() @@ -172,10 +169,10 @@ class TestCarModelBase(unittest.TestCase): cls.can_msgs = sorted(can_msgs, key=lambda msg: msg.logMonoTime) - cls.CarInterface, cls.CarController, cls.CarState = interfaces[cls.car_model] - cls.CP = cls.CarInterface.get_params(cls.car_model, cls.fingerprint, car_fw, experimental_long, docs=False) + cls.CarInterface, cls.CarController, cls.CarState = interfaces[cls.platform] + cls.CP = cls.CarInterface.get_params(cls.platform, cls.fingerprint, car_fw, experimental_long, docs=False) assert cls.CP - assert cls.CP.carFingerprint == cls.car_model + assert cls.CP.carFingerprint == cls.platform os.environ["COMMA_CACHE"] = DEFAULT_DOWNLOAD_CACHE_ROOT @@ -478,7 +475,7 @@ class TestCarModelBase(unittest.TestCase): "This is fine to fail for WIP car ports, just let us know and we can upload your routes to the CI bucket.") -@parameterized_class(('car_model', 'test_route'), get_test_cases()) +@parameterized_class(('platform', 'test_route'), get_test_cases()) @pytest.mark.xdist_group_class_property('test_route') class TestCarModel(TestCarModelBase): pass diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index d960114f1b..8e8e0292f2 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -37,6 +37,7 @@ class CarController(CarControllerBase): self.last_standstill = False self.standstill_req = False self.steer_rate_counter = 0 + self.distance_button = 0 self.packer = CANPacker(dbc_name) self.gas = 0 @@ -139,14 +140,23 @@ class CarController(CarControllerBase): if (self.frame % 3 == 0 and self.CP.openpilotLongitudinalControl) or pcm_cancel_cmd: lead = hud_control.leadVisible or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged + # Press distance button until we are at the correct bar length. Only change while enabled to avoid skipping startup popup + if self.frame % 6 == 0: + if CS.pcm_follow_distance_values.get(CS.pcm_follow_distance, "UNKNOWN") != "FAR" and CS.out.cruiseState.enabled and \ + self.CP.carFingerprint not in UNSUPPORTED_DSU_CAR: + self.distance_button = not self.distance_button + else: + self.distance_button = 0 + # Lexus IS uses a different cancellation message if pcm_cancel_cmd and self.CP.carFingerprint in UNSUPPORTED_DSU_CAR: can_sends.append(toyotacan.create_acc_cancel_command(self.packer)) elif self.CP.openpilotLongitudinalControl: - can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert)) + can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert, + self.distance_button)) self.accel = pcm_accel_cmd else: - can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False)) + can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False, self.distance_button)) if self.frame % 2 == 0 and self.CP.enableGasInterceptor and self.CP.openpilotLongitudinalControl: # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd. diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 166f6735a0..65fced80f4 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -43,6 +43,9 @@ class CarState(CarStateBase): self.prev_distance_button = 0 self.distance_button = 0 + self.pcm_follow_distance = 0 + self.pcm_follow_distance_values = can_define.dv['PCM_CRUISE_2']['PCM_FOLLOW_DISTANCE'] + self.low_speed_lockout = False self.acc_type = 1 self.lkas_hud = {} @@ -166,13 +169,16 @@ class CarState(CarStateBase): if self.CP.carFingerprint != CAR.PRIUS_V: self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"]) - # distance button is wired to the ACC module (camera or radar) - self.prev_distance_button = self.distance_button - if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR): - self.distance_button = cp_acc.vl["ACC_CONTROL"]["DISTANCE"] + if self.CP.carFingerprint not in UNSUPPORTED_DSU_CAR: + self.pcm_follow_distance = cp.vl["PCM_CRUISE_2"]["PCM_FOLLOW_DISTANCE"] - elif self.CP.flags & ToyotaFlags.SMART_DSU and not self.CP.flags & ToyotaFlags.RADAR_CAN_FILTER: - self.distance_button = cp.vl["SDSU"]["FD_BUTTON"] + if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) or (self.CP.flags & ToyotaFlags.SMART_DSU and not self.CP.flags & ToyotaFlags.RADAR_CAN_FILTER): + # distance button is wired to the ACC module (camera or radar) + self.prev_distance_button = self.distance_button + if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR): + self.distance_button = cp_acc.vl["ACC_CONTROL"]["DISTANCE"] + else: + self.distance_button = cp.vl["SDSU"]["FD_BUTTON"] return ret diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index e14e3e53a0..1cc99b41b5 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -33,12 +33,12 @@ def create_lta_steer_command(packer, steer_control_type, steer_angle, steer_req, return packer.make_can_msg("STEERING_LTA", 0, values) -def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, fcw_alert): +def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, fcw_alert, distance): # TODO: find the exact canceling bit that does not create a chime values = { "ACCEL_CMD": accel, "ACC_TYPE": acc_type, - "DISTANCE": 0, + "DISTANCE": distance, "MINI_CAR": lead, "PERMIT_BRAKING": 1, "RELEASE_STANDSTILL": not standstill_req, diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 018768f248..2b1fd01112 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -63,8 +63,8 @@ class LongitudinalPlanner: self.solverExecutionTime = 0.0 self.params = Params() self.param_read_counter = 0 - self.read_param() self.personality = log.LongitudinalPersonality.standard + self.read_param() def read_param(self): try: diff --git a/selfdrive/test/helpers.py b/selfdrive/test/helpers.py index b345b929ec..c157b98b62 100644 --- a/selfdrive/test/helpers.py +++ b/selfdrive/test/helpers.py @@ -88,23 +88,28 @@ def read_segment_list(segment_list_path): return [(platform[2:], segment) for platform, segment in zip(seg_list[::2], seg_list[1::2], strict=True)] +@contextlib.contextmanager +def http_server_context(handler, setup=None): + host = '127.0.0.1' + server = http.server.HTTPServer((host, 0), handler) + port = server.server_port + t = threading.Thread(target=server.serve_forever) + t.start() + + if setup is not None: + setup(host, port) + + try: + yield (host, port) + finally: + server.shutdown() + server.server_close() + t.join() + + def with_http_server(func, handler=http.server.BaseHTTPRequestHandler, setup=None): @wraps(func) def inner(*args, **kwargs): - host = '127.0.0.1' - server = http.server.HTTPServer((host, 0), handler) - port = server.server_port - t = threading.Thread(target=server.serve_forever) - t.start() - - if setup is not None: - setup(host, port) - - try: - return func(*args, f'http://{host}:{port}', **kwargs) - finally: - server.shutdown() - server.server_close() - t.join() - + with http_server_context(handler, setup) as (host, port): + return func(*args, f"http://{host}:{port}", **kwargs) return inner diff --git a/selfdrive/updated/tests/test_updated.py b/selfdrive/updated/tests/test_updated.py index d8ce9f3394..93b6e11383 100755 --- a/selfdrive/updated/tests/test_updated.py +++ b/selfdrive/updated/tests/test_updated.py @@ -21,7 +21,7 @@ def run(args, **kwargs): return subprocess.run(args, **kwargs, check=True) -def update_release(directory, name, version, release_notes): +def update_release(directory, name, version, agnos_version, release_notes): with open(directory / "RELEASES.md", "w") as f: f.write(release_notes) @@ -30,6 +30,9 @@ def update_release(directory, name, version, release_notes): with open(directory / "common" / "version.h", "w") as f: f.write(f'#define COMMA_VERSION "{version}"') + with open(directory / "launch_env.sh", "w") as f: + f.write(f'export AGNOS_VERSION="{agnos_version}"') + run(["git", "add", "."], cwd=directory) run(["git", "commit", "-m", f"openpilot release {version}"], cwd=directory) @@ -60,8 +63,8 @@ class TestUpdateD(unittest.TestCase): os.environ["UPDATER_LOCK_FILE"] = str(self.mock_update_path / "safe_staging_overlay.lock") self.MOCK_RELEASES = { - "release3": ("0.1.2", "0.1.2 release notes"), - "master": ("0.1.3", "0.1.3 release notes"), + "release3": ("0.1.2", "1.2", "0.1.2 release notes"), + "master": ("0.1.3", "1.2", "0.1.3 release notes"), } def set_target_branch(self, branch): @@ -97,7 +100,7 @@ class TestUpdateD(unittest.TestCase): self.assertEqual(self.params.get_bool("UpdaterFetchAvailable"), fetch_available) self.assertEqual(self.params.get_bool("UpdateAvailable"), update_available) - def _test_update_params(self, branch, version, release_notes): + def _test_update_params(self, branch, version, agnos_version, release_notes): self.assertTrue(self.params.get("UpdaterNewDescription", encoding="utf-8").startswith(f"{version} / {branch}")) self.assertEqual(self.params.get("UpdaterNewReleaseNotes", encoding="utf-8"), f"
{release_notes}
\n") @@ -116,6 +119,22 @@ class TestUpdateD(unittest.TestCase): time.sleep(1) + def test_no_update(self): + # Start on release3, ensure we don't fetch any updates + self.setup_remote_release("release3") + self.setup_basedir_release("release3") + + with processes_context(["updated"]) as [updated]: + self._test_params("release3", False, False) + time.sleep(1) + self._test_params("release3", False, False) + + self.send_check_for_updates_signal(updated) + + self.wait_for_idle() + + self._test_params("release3", False, False) + def test_new_release(self): # Start on release3, simulate a release3 commit, ensure we fetch that update properly self.setup_remote_release("release3") @@ -126,7 +145,7 @@ class TestUpdateD(unittest.TestCase): time.sleep(1) self._test_params("release3", False, False) - self.MOCK_RELEASES["release3"] = ("0.1.3", "0.1.3 release notes") + self.MOCK_RELEASES["release3"] = ("0.1.3", "1.2", "0.1.3 release notes") self.update_remote_release("release3") self.send_check_for_updates_signal(updated) @@ -167,6 +186,37 @@ class TestUpdateD(unittest.TestCase): self._test_params("master", False, True) self._test_update_params("master", *self.MOCK_RELEASES["master"]) + def test_agnos_update(self): + # Start on release3, push an update with an agnos change + self.setup_remote_release("release3") + self.setup_basedir_release("release3") + + with mock.patch("openpilot.system.hardware.AGNOS", "True"), \ + mock.patch("openpilot.system.hardware.tici.hardware.Tici.get_os_version", "1.2"), \ + mock.patch("openpilot.system.hardware.tici.agnos.get_target_slot_number"), \ + mock.patch("openpilot.system.hardware.tici.agnos.flash_agnos_update"), \ + processes_context(["updated"]) as [updated]: + + self._test_params("release3", False, False) + time.sleep(1) + self._test_params("release3", False, False) + + self.MOCK_RELEASES["release3"] = ("0.1.3", "1.3", "0.1.3 release notes") + self.update_remote_release("release3") + + self.send_check_for_updates_signal(updated) + + self.wait_for_idle() + + self._test_params("release3", True, False) + + self.send_download_signal(updated) + + self.wait_for_idle() + + self._test_params("release3", False, True) + self._test_update_params("release3", *self.MOCK_RELEASES["release3"]) + if __name__ == "__main__": unittest.main() diff --git a/system/loggerd/uploader.py b/system/loggerd/uploader.py index 33ee8c1850..832a227798 100755 --- a/system/loggerd/uploader.py +++ b/system/loggerd/uploader.py @@ -44,7 +44,9 @@ class FakeResponse: def get_directory_sort(d: str) -> list[str]: - return [s.rjust(10, '0') for s in d.rsplit('--', 1)] + # ensure old format is sorted sooner + o = ["0", ] if d.startswith("2024-") else ["1", ] + return o + [s.rjust(10, '0') for s in d.rsplit('--', 1)] def listdir_by_creation(d: str) -> list[str]: if not os.path.isdir(d): diff --git a/system/qcomgpsd/cgpsd.py b/system/qcomgpsd/cgpsd.py new file mode 100755 index 0000000000..c0edc721fa --- /dev/null +++ b/system/qcomgpsd/cgpsd.py @@ -0,0 +1,119 @@ +#!/usr/bin/env python3 +import time +import datetime +from collections import defaultdict + +from cereal import log +import cereal.messaging as messaging +from openpilot.common.swaglog import cloudlog +from openpilot.system.qcomgpsd.qcomgpsd import at_cmd, wait_for_modem + +# https://campar.in.tum.de/twiki/pub/Chair/NaviGpsDemon/nmea.html#RMC +""" +AT+CGPSGPOS=1 +response: '$GNGGA,220212.00,3245.09188,N,11711.76362,W,1,06,24.54,0.0,M,,M,,*77' + +AT+CGPSGPOS=2 +response: '$GNGSA,A,3,06,17,19,22,,,,,,,,,14.11,8.95,10.91,1*01 +$GNGSA,A,3,29,26,,,,,,,,,,,14.11,8.95,10.91,4*03' + +AT+CGPSGPOS=3 +response: '$GPGSV,3,1,11,06,55,047,22,19,29,053,20,22,19,115,14,05,01,177,,0*68 +$GPGSV,3,2,11,11,77,156,23,12,47,322,17,17,08,066,10,20,25,151,,0*6D +$GPGSV,3,3,11,24,44,232,,25,16,312,,29,02,260,,0*5D' + +AT+CGPSGPOS=4 +response: '$GBGSV,1,1,03,26,75,242,20,29,19,049,16,35,,,24,0*7D' + +AT+CGPSGPOS=5 +response: '$GNRMC,220216.00,A,3245.09531,N,11711.76043,W,,,070324,,,A,V*20' +""" + + +def sfloat(n: str): + return float(n) if len(n) > 0 else 0 + +def checksum(s: str): + ret = 0 + for c in s[1:-3]: + ret ^= ord(c) + return format(ret, '02X') + +def main(): + wait_for_modem("AT+CGPS?") + + cmds = [ + "AT+GPSPORT=1", + "AT+CGPS=1", + ] + for c in cmds: + at_cmd(c) + + nmea = defaultdict(list) + pm = messaging.PubMaster(['gpsLocation']) + while True: + time.sleep(1) + try: + # TODO: read from streaming AT port instead of polling + out = at_cmd("AT+CGPS?") + + sentences = out.split("'")[1].splitlines() + new = {l.split(',')[0]: l.split(',') for l in sentences if l.startswith('$G')} + nmea.update(new) + if '$GNRMC' not in new: + print(f"no GNRMC:\n{out}\n") + continue + + # validate checksums + for s in nmea.values(): + sent = ','.join(s) + if checksum(sent) != s[-1].split('*')[1]: + cloudlog.error(f"invalid checksum: {repr(sent)}") + continue + + gnrmc = nmea['$GNRMC'] + #print(gnrmc) + + msg = messaging.new_message('gpsLocation', valid=True) + gps = msg.gpsLocation + gps.latitude = (sfloat(gnrmc[3][:2]) + (sfloat(gnrmc[3][2:]) / 60)) * (1 if gnrmc[4] == "N" else -2) + gps.longitude = (sfloat(gnrmc[5][:3]) + (sfloat(gnrmc[5][3:]) / 60)) * (1 if gnrmc[6] == "E" else -1) + + date = gnrmc[9][:6] + dt = datetime.datetime.strptime(f"{date} {gnrmc[1]}", '%d%m%y %H%M%S.%f') + gps.unixTimestampMillis = dt.timestamp()*1e3 + + gps.flags = 1 if gnrmc[1] == 'A' else 0 + + # TODO: make our own source + gps.source = log.GpsLocationData.SensorSource.qcomdiag + + gps.speed = sfloat(gnrmc[7]) + gps.bearingDeg = sfloat(gnrmc[8]) + + if len(nmea['$GNGGA']): + gngga = nmea['$GNGGA'] + if gngga[10] == 'M': + gps.altitude = sfloat(gngga[9]) + + if len(nmea['$GNGSA']): + # TODO: this is only for GPS sats + gngsa = nmea['$GNGSA'] + gps.horizontalAccuracy = sfloat(gngsa[4]) + gps.verticalAccuracy = sfloat(gngsa[5]) + + # TODO: set these from the module + gps.bearingAccuracyDeg = 5. + gps.speedAccuracy = 3. + + # TODO: can we get this from the NMEA sentences? + #gps.vNED = vNED + + pm.send('gpsLocation', msg) + + except Exception: + cloudlog.exception("gps.issue") + + +if __name__ == "__main__": + main() diff --git a/system/qcomgpsd/qcomgpsd.py b/system/qcomgpsd/qcomgpsd.py index e8c407a627..25b547cf5e 100755 --- a/system/qcomgpsd/qcomgpsd.py +++ b/system/qcomgpsd/qcomgpsd.py @@ -205,10 +205,10 @@ def teardown_quectel(diag): try_setup_logs(diag, []) -def wait_for_modem(): +def wait_for_modem(cmd="AT+QGPS?"): cloudlog.warning("waiting for modem to come up") while True: - ret = subprocess.call("mmcli -m any --timeout 10 --command=\"AT+QGPS?\"", stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, shell=True) + ret = subprocess.call(f"mmcli -m any --timeout 10 --command=\"{cmd}\"", stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, shell=True) if ret == 0: return time.sleep(0.1) diff --git a/tools/car_porting/test_car_model.py b/tools/car_porting/test_car_model.py index 86980b054b..1dfac7dcf3 100755 --- a/tools/car_porting/test_car_model.py +++ b/tools/car_porting/test_car_model.py @@ -5,6 +5,7 @@ import unittest from openpilot.selfdrive.car.tests.routes import CarTestRoute from openpilot.selfdrive.car.tests.test_models import TestCarModel +from openpilot.selfdrive.car.values import PLATFORMS from openpilot.tools.lib.route import SegmentName @@ -31,7 +32,10 @@ if __name__ == "__main__": route_or_segment_name = SegmentName(args.route_or_segment_name.strip(), allow_route_name=True) segment_num = route_or_segment_name.segment_num if route_or_segment_name.segment_num != -1 else None - test_route = CarTestRoute(route_or_segment_name.route_name.canonical_name, args.car, segment=segment_num) + + platform = PLATFORMS.get(args.car) + + test_route = CarTestRoute(route_or_segment_name.route_name.canonical_name, platform, segment=segment_num) test_suite = create_test_models_suite([test_route], ci=args.ci) unittest.TextTestRunner().run(test_suite) diff --git a/tools/lib/logreader.py b/tools/lib/logreader.py index 6247bbc9db..7a1e972e19 100755 --- a/tools/lib/logreader.py +++ b/tools/lib/logreader.py @@ -248,7 +248,7 @@ are uploaded or auto fallback to qlogs with '/a' selector at the end of the rout def _get_lr(self, i): if i not in self.__lrs: - self.__lrs[i] = _LogFileReader(self.logreader_identifiers[i]) + self.__lrs[i] = _LogFileReader(self.logreader_identifiers[i], sort_by_time=self.sort_by_time, only_union_types=self.only_union_types) return self.__lrs[i] def __iter__(self): diff --git a/tools/lib/tests/test_logreader.py b/tools/lib/tests/test_logreader.py index 2141915b87..fc72202b26 100755 --- a/tools/lib/tests/test_logreader.py +++ b/tools/lib/tests/test_logreader.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +import capnp import contextlib import io import shutil @@ -11,6 +12,7 @@ import requests from parameterized import parameterized from unittest import mock +from cereal import log as capnp_log from openpilot.tools.lib.logreader import LogIterable, LogReader, comma_api_source, parse_indirect, ReadMode, InternalUnavailableException from openpilot.tools.lib.route import SegmentRange from openpilot.tools.lib.url_file import URLFileException @@ -216,6 +218,43 @@ class TestLogReader(unittest.TestCase): log_len = len(list(lr)) self.assertEqual(qlog_len, log_len) + @pytest.mark.slow + def test_sort_by_time(self): + msgs = list(LogReader(f"{TEST_ROUTE}/0/q")) + self.assertNotEqual(msgs, sorted(msgs, key=lambda m: m.logMonoTime)) + + msgs = list(LogReader(f"{TEST_ROUTE}/0/q", sort_by_time=True)) + self.assertEqual(msgs, sorted(msgs, key=lambda m: m.logMonoTime)) + + def test_only_union_types(self): + with tempfile.NamedTemporaryFile() as qlog: + # write valid Event messages + num_msgs = 100 + with open(qlog.name, "wb") as f: + f.write(b"".join(capnp_log.Event.new_message().to_bytes() for _ in range(num_msgs))) + + msgs = list(LogReader(qlog.name)) + self.assertEqual(len(msgs), num_msgs) + [m.which() for m in msgs] + + # append non-union Event message + event_msg = capnp_log.Event.new_message() + non_union_bytes = bytearray(event_msg.to_bytes()) + non_union_bytes[event_msg.total_size.word_count * 8] = 0xff # set discriminant value out of range using Event word offset + with open(qlog.name, "ab") as f: + f.write(non_union_bytes) + + # ensure new message is added, but is not a union type + msgs = list(LogReader(qlog.name)) + self.assertEqual(len(msgs), num_msgs + 1) + with self.assertRaises(capnp.KjException): + [m.which() for m in msgs] + + # should not be added when only_union_types=True + msgs = list(LogReader(qlog.name, only_union_types=True)) + self.assertEqual(len(msgs), num_msgs) + [m.which() for m in msgs] + if __name__ == "__main__": unittest.main() diff --git a/tools/replay/route.cc b/tools/replay/route.cc index d0ddf7f3c8..d5847a94b8 100644 --- a/tools/replay/route.cc +++ b/tools/replay/route.cc @@ -18,7 +18,7 @@ Route::Route(const QString &route, const QString &data_dir) : data_dir_(data_dir } RouteIdentifier Route::parseRoute(const QString &str) { - QRegExp rx(R"(^(?:([a-z0-9]{16})([|_/]))?(\d{4}-\d{2}-\d{2}--\d{2}-\d{2}-\d{2})(?:(--|/)(\d*))?$)"); + QRegExp rx(R"(^(?:([a-z0-9]{16})([|_/]))?(.{20})(?:(--|/)(\d*))?$)"); if (rx.indexIn(str) == -1) return {}; const QStringList list = rx.capturedTexts();