diff --git a/cereal b/cereal index 2335f98bb..bd2f7fa56 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 2335f98bbe628ec6fde92c8d929ecaf373b125af +Subproject commit bd2f7fa56706bcec3c9906bd57c2ec46f0666ac5 diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index 1276c8518..7998e65c2 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -12,6 +12,14 @@ ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName +def apply_hysteresis(val: float, val_steady: float, hyst_gap: float) -> float: + if val > val_steady + hyst_gap: + val_steady = val - hyst_gap + elif val < val_steady - hyst_gap: + val_steady = val + hyst_gap + return val_steady + + def create_button_event(cur_but: int, prev_but: int, buttons_dict: Dict[int, capnp.lib.capnp._EnumModule], unpressed: int = 0) -> capnp.lib.capnp._DynamicStructBuilder: if cur_but != unpressed: diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 4696bec82..a37667fd3 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -24,6 +24,7 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg): ("MOTOR_TORQUE", "STEER_MOTOR_TORQUE"), ("STEER_TORQUE_SENSOR", "STEER_STATUS"), ("IMPERIAL_UNIT", "CAR_SPEED"), + ("ROUGH_CAR_SPEED_2", "CAR_SPEED"), ("LEFT_BLINKER", "SCM_FEEDBACK"), ("RIGHT_BLINKER", "SCM_FEEDBACK"), ("SEATBELT_DRIVER_LAMP", "SEATBELT_STATUS"), @@ -150,6 +151,10 @@ class CarState(CarStateBase): self.cruise_setting = 0 self.v_cruise_pcm_prev = 0 + # When available we use cp.vl["CAR_SPEED"]["ROUGH_CAR_SPEED_2"] to populate vEgoCluster + # However, on cars without a digital speedometer this is not always present (HRV, FIT, CRV 2016, ILX and RDX) + self.dash_speed_seen = False + def update(self, cp, cp_cam, cp_body): ret = car.CarState.new_message() @@ -203,6 +208,11 @@ class CarState(CarStateBase): ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] * CV.KPH_TO_MS * self.CP.wheelSpeedFactor + v_weight * v_wheel ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + self.dash_speed_seen = self.dash_speed_seen or cp.vl["CAR_SPEED"]["ROUGH_CAR_SPEED_2"] > 1e-3 + if self.dash_speed_seen: + conversion = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS + ret.vEgoCluster = cp.vl["CAR_SPEED"]["ROUGH_CAR_SPEED_2"] * conversion + ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE"] ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE_RATE"] @@ -237,9 +247,9 @@ class CarState(CarStateBase): ret.cruiseState.standstill = acc_hud["CRUISE_SPEED"] == 252. # on certain cars, CRUISE_SPEED changes to imperial with car's unit setting - conversion_factor = CV.MPH_TO_MS if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS and not self.is_metric else CV.KPH_TO_MS + conversion = CV.MPH_TO_MS if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS and not self.is_metric else CV.KPH_TO_MS # On set, cruise set speed pulses between 254~255 and the set speed prev is set to avoid this. - ret.cruiseState.speed = self.v_cruise_pcm_prev if acc_hud["CRUISE_SPEED"] > 160.0 else acc_hud["CRUISE_SPEED"] * conversion_factor + ret.cruiseState.speed = self.v_cruise_pcm_prev if acc_hud["CRUISE_SPEED"] > 160.0 else acc_hud["CRUISE_SPEED"] * conversion self.v_cruise_pcm_prev = ret.cruiseState.speed else: ret.cruiseState.speed = cp.vl["CRUISE"]["CRUISE_SPEED_PCM"] * CV.KPH_TO_MS diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index d02db2673..8164ac66d 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -9,7 +9,7 @@ from common.basedir import BASEDIR from common.conversions import Conversions as CV from common.kalman.simple_kalman import KF1D from common.realtime import DT_CTRL -from selfdrive.car import create_button_enable_events, gen_empty_fingerprint +from selfdrive.car import apply_hysteresis, create_button_enable_events, gen_empty_fingerprint from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX from selfdrive.controls.lib.events import Events from selfdrive.controls.lib.vehicle_model import VehicleModel @@ -172,6 +172,12 @@ class CarInterfaceBase(ABC): else: self.v_ego_cluster_seen = True + # Many cars apply hysteresis to the ego dash speed + if self.CS is not None: + ret.vEgoCluster = apply_hysteresis(ret.vEgoCluster, self.CS.out.vEgoCluster, self.CS.cluster_speed_hyst_gap) + if abs(ret.vEgo) < self.CS.cluster_min_speed: + ret.vEgoCluster = 0.0 + if ret.cruiseState.speedCluster == 0: ret.cruiseState.speedCluster = ret.cruiseState.speed @@ -217,6 +223,8 @@ class CarInterfaceBase(ABC): events.add(EventName.parkBrake) if cs_out.accFaulted: events.add(EventName.accFaulted) + if cs_out.steeringPressed: + events.add(EventName.steerOverride) # Handle button presses events.events.extend(create_button_enable_events(cs_out.buttonEvents, enable_buttons, pcm_cruise=self.CP.pcmCruise)) @@ -272,6 +280,8 @@ class CarStateBase(ABC): self.right_blinker_cnt = 0 self.left_blinker_prev = False self.right_blinker_prev = False + self.cluster_speed_hyst_gap = 0.0 + self.cluster_min_speed = 0.0 # min speed before dropping to 0 # Q = np.matrix([[0.0, 0.0], [0.0, 100.0]]) # R = 0.3 diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py index 030a368a1..7648a4a50 100644 --- a/selfdrive/car/tesla/values.py +++ b/selfdrive/car/tesla/values.py @@ -22,7 +22,7 @@ CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = { FINGERPRINTS = { CAR.AP2_MODELS: [ { - 1: 8, 3: 8, 14: 8, 21: 4, 69: 8, 109: 4, 257: 3, 264: 8, 277: 6, 280: 6, 293: 4, 296: 4, 309: 5, 325: 8, 328: 5, 336: 8, 341: 8, 360: 7, 373: 8, 389: 8, 415: 8, 513: 5, 516: 8, 518: 8, 520: 4, 522: 8, 524: 8, 526: 8, 532: 3, 536: 8, 537: 3, 538: 8, 542: 8, 551: 5, 552: 2, 556: 8, 558: 8, 568: 8, 569: 8, 574: 8, 576: 3, 577: 8, 582: 5, 583: 8, 584: 4, 585: 8, 590: 8, 601: 8, 606: 8, 608: 1, 622: 8, 627: 6, 638: 8, 641: 8, 643: 8, 692: 8, 693: 8, 695: 8, 696: 8, 697: 8, 699: 8, 700: 8, 701: 8, 702: 8, 703: 8, 704: 8, 708: 8, 709: 8, 710: 8, 711: 8, 712: 8, 728: 8, 744: 8, 760: 8, 772: 8, 775: 8, 776: 8, 777: 8, 778: 8, 782: 8, 788: 8, 791: 8, 792: 8, 796: 2, 797: 8, 798: 6, 799: 8, 804: 8, 805: 8, 807: 8, 808: 1, 811: 8, 812: 8, 813: 8, 814: 5, 815: 8, 820: 8, 823: 8, 824: 8, 829: 8, 830: 5, 836: 8, 840: 8, 845: 8, 846: 5, 848: 8, 852: 8, 853: 8, 856: 4, 857: 6, 861: 8, 862: 5, 872: 8, 876: 8, 877: 8, 879: 8, 880: 8, 882: 8, 884: 8, 888: 8, 893: 8, 894: 8, 901: 6, 904: 3, 905: 8, 906: 8, 908: 2, 909: 8, 910: 8, 912: 8, 920: 8, 921: 8, 925: 4, 926: 6, 936: 8, 941: 8, 949: 8, 952: 8, 953: 6, 968: 8, 969: 7, 970: 8, 971: 8, 977: 8, 984: 8, 987: 8, 990: 8, 1000: 8, 1001: 8, 1006: 8, 1007: 8, 1008: 8, 1010: 6, 1014: 1, 1015: 8, 1016: 8, 1017: 8, 1018: 8, 1020: 8, 1026: 8, 1028: 8, 1029: 8, 1030: 8, 1032: 1, 1033: 1, 1034: 8, 1048: 1, 1049: 8, 1061: 8, 1064: 8, 1065: 8, 1070: 8, 1080: 8, 1081: 8, 1097: 8, 1113: 8, 1129: 8, 1145: 8, 1160: 4, 1177: 8, 1281: 8, 1328: 8, 1329: 8, 1332: 8, 1335: 8, 1337: 8, 1353: 8, 1368: 8, 1412: 8, 1436: 8, 1476: 8, 1481: 8, 1497: 8, 1513: 8, 1519: 8, 1601: 8, 1605: 8, 1617: 8, 1621: 8, 1625: 8, 1665: 8, 1792: 8, 1798: 8, 1800: 4, 1804: 8, 1812: 8, 1815: 8, 1816: 8, 1824: 8, 1825: 8, 1828: 8, 1831: 8, 1832: 8, 1840: 8, 1842: 8, 1848: 8, 1864: 8, 1872: 8, 1880: 8, 1888: 8, 1892: 8, 1896: 8, 1912: 8, 1937: 8, 1953: 8, 1960: 8, 1968: 8, 1992: 8, 2001: 8, 2008: 3, 2015: 8, 2016: 8, 2043: 5, 2045: 4 + 1: 8, 3: 8, 14: 8, 21: 4, 69: 8, 109: 4, 257: 3, 264: 8, 277: 6, 280: 6, 293: 4, 296: 4, 309: 5, 325: 8, 328: 5, 336: 8, 341: 8, 360: 7, 373: 8, 389: 8, 415: 8, 513: 5, 516: 8, 518: 8, 520: 4, 522: 8, 524: 8, 526: 8, 532: 3, 536: 8, 537: 3, 538: 8, 542: 8, 551: 5, 552: 2, 556: 8, 558: 8, 568: 8, 569: 8, 574: 8, 576: 3, 577: 8, 582: 5, 583: 8, 584: 4, 585: 8, 590: 8, 601: 8, 606: 8, 608: 1, 622: 8, 627: 6, 638: 8, 641: 8, 643: 8, 692: 8, 693: 8, 695: 8, 696: 8, 697: 8, 699: 8, 700: 8, 701: 8, 702: 8, 703: 8, 704: 8, 708: 8, 709: 8, 710: 8, 711: 8, 712: 8, 728: 8, 744: 8, 760: 8, 772: 8, 775: 8, 776: 8, 777: 8, 778: 8, 782: 8, 788: 8, 791: 8, 792: 8, 796: 2, 797: 8, 798: 6, 799: 8, 804: 8, 805: 8, 807: 8, 808: 1, 811: 8, 812: 8, 813: 8, 814: 5, 815: 8, 820: 8, 823: 8, 824: 8, 829: 8, 830: 5, 836: 8, 840: 8, 845: 8, 846: 5, 848: 8, 852: 8, 853: 8, 856: 4, 857: 6, 861: 8, 862: 5, 872: 8, 876: 8, 877: 8, 879: 8, 880: 8, 882: 8, 884: 8, 888: 8, 893: 8, 894: 8, 901: 6, 904: 3, 905: 8, 906: 8, 908: 2, 909: 8, 910: 8, 912: 8, 920: 8, 921: 8, 925: 4, 926: 6, 936: 8, 941: 8, 949: 8, 952: 8, 953: 6, 968: 8, 969: 7, 970: 8, 971: 8, 977: 8, 984: 8, 986: 8, 987: 8, 990: 8, 1000: 8, 1001: 8, 1006: 8, 1007: 8, 1008: 8, 1010: 6, 1014: 1, 1015: 8, 1016: 8, 1017: 8, 1018: 8, 1020: 8, 1026: 8, 1028: 8, 1029: 8, 1030: 8, 1032: 1, 1033: 1, 1034: 8, 1048: 1, 1049: 8, 1061: 8, 1064: 8, 1065: 8, 1070: 8, 1080: 8, 1081: 8, 1097: 8, 1113: 8, 1129: 8, 1145: 8, 1160: 4, 1177: 8, 1281: 8, 1328: 8, 1329: 8, 1332: 8, 1335: 8, 1337: 8, 1353: 8, 1368: 8, 1412: 8, 1436: 8, 1476: 8, 1481: 8, 1497: 8, 1513: 8, 1519: 8, 1601: 8, 1605: 8, 1617: 8, 1621: 8, 1625: 8, 1665: 8, 1792: 8, 1798: 8, 1800: 4, 1804: 8, 1812: 8, 1815: 8, 1816: 8, 1824: 8, 1825: 8, 1828: 8, 1831: 8, 1832: 8, 1840: 8, 1842: 8, 1848: 8, 1864: 8, 1872: 8, 1880: 8, 1888: 8, 1892: 8, 1896: 8, 1912: 8, 1937: 8, 1953: 8, 1960: 8, 1968: 8, 1992: 8, 2001: 8, 2008: 3, 2015: 8, 2016: 8, 2043: 5, 2045: 4 }, ], CAR.AP1_MODELS: [ diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 453b859ae..1c6e4922d 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -39,6 +39,7 @@ class CarInterface(CarInterfaceBase): if any(msg in fingerprint[1] for msg in (0x1A0, 0xC2)): # Bremse_1, Lenkwinkel_1 ret.networkLocation = NetworkLocation.gateway + ret.experimentalLongitudinalAvailable = True else: ret.networkLocation = NetworkLocation.fwdCamera @@ -64,6 +65,7 @@ class CarInterface(CarInterfaceBase): if any(msg in fingerprint[1] for msg in (0x40, 0x86, 0xB2, 0xFD)): # Airbag_01, LWI_01, ESP_19, ESP_21 ret.networkLocation = NetworkLocation.gateway + ret.experimentalLongitudinalAvailable = True else: ret.networkLocation = NetworkLocation.fwdCamera diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 569c12e3d..308753529 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -508,9 +508,9 @@ class Controls: self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL) self.current_alert_types.append(ET.SOFT_DISABLE) - elif self.events.any(ET.OVERRIDE): + elif self.events.any(ET.OVERRIDE_LATERAL) or self.events.any(ET.OVERRIDE_LONGITUDINAL): self.state = State.overriding - self.current_alert_types.append(ET.OVERRIDE) + self.current_alert_types += [ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL] # SOFT DISABLING elif self.state == State.softDisabling: @@ -540,10 +540,10 @@ class Controls: self.state = State.softDisabling self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL) self.current_alert_types.append(ET.SOFT_DISABLE) - elif not self.events.any(ET.OVERRIDE): + elif not (self.events.any(ET.OVERRIDE_LATERAL) or self.events.any(ET.OVERRIDE_LONGITUDINAL)): self.state = State.enabled else: - self.current_alert_types.append(ET.OVERRIDE) + self.current_alert_types += [ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL] # DISABLED elif self.state == State.disabled: @@ -554,7 +554,7 @@ class Controls: else: if self.events.any(ET.PRE_ENABLE): self.state = State.preEnabled - elif self.events.any(ET.OVERRIDE): + elif self.events.any(ET.OVERRIDE_LATERAL) or self.events.any(ET.OVERRIDE_LONGITUDINAL): self.state = State.overriding else: self.state = State.enabled @@ -586,7 +586,7 @@ class Controls: # Check which actuators can be enabled CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ CS.vEgo > self.CP.minSteerSpeed and not CS.standstill - CC.longActive = self.active and not self.events.any(ET.OVERRIDE) and self.CP.openpilotLongitudinalControl + CC.longActive = self.active and not self.events.any(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl actuators = CC.actuators actuators.longControlState = self.LoC.long_control_state diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 5139ead84..91f1748ec 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -31,7 +31,8 @@ class Priority(IntEnum): class ET: ENABLE = 'enable' PRE_ENABLE = 'preEnable' - OVERRIDE = 'override' + OVERRIDE_LATERAL = 'overrideLateral' + OVERRIDE_LONGITUDINAL = 'overrideLongitudinal' NO_ENTRY = 'noEntry' WARNING = 'warning' USER_DISABLE = 'userDisable' @@ -623,7 +624,15 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { }, EventName.gasPressedOverride: { - ET.OVERRIDE: Alert( + ET.OVERRIDE_LONGITUDINAL: Alert( + "", + "", + AlertStatus.normal, AlertSize.none, + Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .1), + }, + + EventName.steerOverride: { + ET.OVERRIDE_LATERAL: Alert( "", "", AlertStatus.normal, AlertSize.none, diff --git a/selfdrive/controls/tests/test_state_machine.py b/selfdrive/controls/tests/test_state_machine.py index 244c56687..36535dfda 100755 --- a/selfdrive/controls/tests/test_state_machine.py +++ b/selfdrive/controls/tests/test_state_machine.py @@ -11,11 +11,11 @@ from selfdrive.controls.lib.events import Events, ET, Alert, Priority, AlertSize State = log.ControlsState.OpenpilotState # The event types that maintain the current state -MAINTAIN_STATES = {State.enabled: None, State.disabled: None, State.softDisabling: ET.SOFT_DISABLE, - State.preEnabled: ET.PRE_ENABLE, State.overriding: ET.OVERRIDE} +MAINTAIN_STATES = {State.enabled: (None,), State.disabled: (None,), State.softDisabling: (ET.SOFT_DISABLE,), + State.preEnabled: (ET.PRE_ENABLE,), State.overriding: (ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL)} ALL_STATES = tuple(State.schema.enumerants.values()) # The event types checked in DISABLED section of state machine -ENABLE_EVENT_TYPES = (ET.ENABLE, ET.PRE_ENABLE, ET.OVERRIDE) +ENABLE_EVENT_TYPES = (ET.ENABLE, ET.PRE_ENABLE, ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL) def make_event(event_types): @@ -41,29 +41,32 @@ class TestStateMachine(unittest.TestCase): def test_immediate_disable(self): for state in ALL_STATES: - self.controlsd.events.add(make_event([MAINTAIN_STATES[state], ET.IMMEDIATE_DISABLE])) - self.controlsd.state = state - self.controlsd.state_transition(self.CS) - self.assertEqual(State.disabled, self.controlsd.state) - self.controlsd.events.clear() + for et in MAINTAIN_STATES[state]: + self.controlsd.events.add(make_event([et, ET.IMMEDIATE_DISABLE])) + self.controlsd.state = state + self.controlsd.state_transition(self.CS) + self.assertEqual(State.disabled, self.controlsd.state) + self.controlsd.events.clear() def test_user_disable(self): for state in ALL_STATES: - self.controlsd.events.add(make_event([MAINTAIN_STATES[state], ET.USER_DISABLE])) - self.controlsd.state = state - self.controlsd.state_transition(self.CS) - self.assertEqual(State.disabled, self.controlsd.state) - self.controlsd.events.clear() + for et in MAINTAIN_STATES[state]: + self.controlsd.events.add(make_event([et, ET.USER_DISABLE])) + self.controlsd.state = state + self.controlsd.state_transition(self.CS) + self.assertEqual(State.disabled, self.controlsd.state) + self.controlsd.events.clear() def test_soft_disable(self): for state in ALL_STATES: if state == State.preEnabled: # preEnabled considers NO_ENTRY instead continue - self.controlsd.events.add(make_event([MAINTAIN_STATES[state], ET.SOFT_DISABLE])) - self.controlsd.state = state - self.controlsd.state_transition(self.CS) - self.assertEqual(self.controlsd.state, State.disabled if state == State.disabled else State.softDisabling) - self.controlsd.events.clear() + for et in MAINTAIN_STATES[state]: + self.controlsd.events.add(make_event([et, ET.SOFT_DISABLE])) + self.controlsd.state = state + self.controlsd.state_transition(self.CS) + self.assertEqual(self.controlsd.state, State.disabled if state == State.disabled else State.softDisabling) + self.controlsd.events.clear() def test_soft_disable_timer(self): self.controlsd.state = State.enabled @@ -93,11 +96,12 @@ class TestStateMachine(unittest.TestCase): def test_maintain_states(self): # Given current state's event type, we should maintain state for state in ALL_STATES: - self.controlsd.state = state - self.controlsd.events.add(make_event([MAINTAIN_STATES[state]])) - self.controlsd.state_transition(self.CS) - self.assertEqual(self.controlsd.state, state) - self.controlsd.events.clear() + for et in MAINTAIN_STATES[state]: + self.controlsd.state = state + self.controlsd.events.add(make_event([et])) + self.controlsd.state_transition(self.CS) + self.assertEqual(self.controlsd.state, state) + self.controlsd.events.clear() if __name__ == "__main__": diff --git a/selfdrive/sensord/tests/test_sensord.py b/selfdrive/sensord/tests/test_sensord.py index 6cb2fa9e6..84582518f 100755 --- a/selfdrive/sensord/tests/test_sensord.py +++ b/selfdrive/sensord/tests/test_sensord.py @@ -116,13 +116,16 @@ class TestSensord(unittest.TestCase): # make sure gpiochip0 is readable HARDWARE.initialize_hardware() - @with_processes(['sensord']) + # read initial sensor values every test case can use + managed_processes["sensord"].start() + cls.events = read_sensor_events(5) + managed_processes["sensord"].stop() + def test_sensors_present(self): # verify correct sensors configuration - events = read_sensor_events(10) seen = set() - for event in events: + for event in self.events: for measurement in event.sensorEvents: # filter unset events (bmx magn) if measurement.version == 0: @@ -131,13 +134,11 @@ class TestSensord(unittest.TestCase): self.assertIn(seen, SENSOR_CONFIGURATIONS) - @with_processes(['sensord']) def test_lsm6ds3_100Hz(self): # verify measurements are sampled and published at a 100Hz rate - events = read_sensor_events(3) # 3sec (about 300 measurements) data_points = set() - for event in events: + for event in self.events: for measurement in event.sensorEvents: # skip lsm6ds3 temperature measurements @@ -162,13 +163,11 @@ class TestSensord(unittest.TestCase): stddev = np.std(tdiffs) assert stddev < 1.5*10**6, f"Standard-dev to big {stddev}" - @with_processes(['sensord']) def test_events_check(self): # verify if all sensors produce events - events = read_sensor_events(3) sensor_events = dict() - for event in events: + for event in self.events: for measurement in event.sensorEvents: # filter unset events (bmx magn) @@ -184,13 +183,11 @@ class TestSensord(unittest.TestCase): err_msg = f"Sensor {s}: 200 < {sensor_events[s]}" assert sensor_events[s] > 200, err_msg - @with_processes(['sensord']) def test_logmonottime_timestamp_diff(self): # ensure diff between the message logMonotime and sample timestamp is small - events = read_sensor_events(3) tdiffs = list() - for event in events: + for event in self.events: for measurement in event.sensorEvents: # filter unset events (bmx magn) @@ -289,4 +286,3 @@ class TestSensord(unittest.TestCase): if __name__ == "__main__": unittest.main() - diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index f46f39dd2..d0c769a0b 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -48db2dee177706285226d1287912e191f1699865 +3ad478bf44f50815d05acc5b12ff2f01a6cb42ff diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index c5fc0395d..55e1ef161 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -29,7 +29,7 @@ PROCS = { "selfdrive.controls.plannerd": 11.7, "./_ui": 19.2, "selfdrive.locationd.paramsd": 9.0, - "./_sensord": 6.17, + "./_sensord": 12.0, "selfdrive.controls.radard": 4.5, "./_modeld": 4.48, "./boardd": 3.63, diff --git a/selfdrive/ui/qt/offroad/networking.cc b/selfdrive/ui/qt/offroad/networking.cc index c7341d198..7ec8691fe 100644 --- a/selfdrive/ui/qt/offroad/networking.cc +++ b/selfdrive/ui/qt/offroad/networking.cc @@ -207,9 +207,12 @@ WifiUI::WifiUI(QWidget *parent, WifiManager* wifi) : QWidget(parent), wifi(wifi) checkmark = QPixmap(ASSET_PATH + "offroad/icon_checkmark.svg").scaledToWidth(49, Qt::SmoothTransformation); circled_slash = QPixmap(ASSET_PATH + "img_circled_slash.svg").scaledToWidth(49, Qt::SmoothTransformation); - QLabel *scanning = new QLabel(tr("Scanning for networks...")); - scanning->setStyleSheet("font-size: 65px;"); - main_layout->addWidget(scanning, 0, Qt::AlignCenter); + scanningLabel = new QLabel(tr("Scanning for networks...")); + scanningLabel->setStyleSheet("font-size: 65px;"); + main_layout->addWidget(scanningLabel, 0, Qt::AlignCenter); + + list_layout = new QVBoxLayout; + main_layout->addLayout(list_layout); setStyleSheet(R"( QScrollBar::handle:vertical { @@ -257,14 +260,12 @@ WifiUI::WifiUI(QWidget *parent, WifiManager* wifi) : QWidget(parent), wifi(wifi) void WifiUI::refresh() { // TODO: don't rebuild this every time - clearLayout(main_layout); + clearLayout(list_layout); + + bool is_empty = wifi->seenNetworks.isEmpty(); + scanningLabel->setVisible(is_empty); + if (is_empty) return; - if (wifi->seenNetworks.size() == 0) { - QLabel *scanning = new QLabel(tr("Scanning for networks...")); - scanning->setStyleSheet("font-size: 65px;"); - main_layout->addWidget(scanning, 0, Qt::AlignCenter); - return; - } QList sortedNetworks = wifi->seenNetworks.values(); std::sort(sortedNetworks.begin(), sortedNetworks.end(), compare_by_strength); @@ -327,6 +328,6 @@ void WifiUI::refresh() { list->addItem(hlayout); } - main_layout->addWidget(list); - main_layout->addStretch(1); + list_layout->addWidget(list); + list_layout->addStretch(1); } diff --git a/selfdrive/ui/qt/offroad/networking.h b/selfdrive/ui/qt/offroad/networking.h index e78d65ef0..4fc9a53d9 100644 --- a/selfdrive/ui/qt/offroad/networking.h +++ b/selfdrive/ui/qt/offroad/networking.h @@ -17,6 +17,8 @@ public: private: WifiManager *wifi = nullptr; + QVBoxLayout *list_layout = nullptr; + QLabel *scanningLabel = nullptr; QVBoxLayout* main_layout; QPixmap lock; QPixmap checkmark; diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h index 016522b05..081483b64 100644 --- a/selfdrive/ui/qt/widgets/cameraview.h +++ b/selfdrive/ui/qt/widgets/cameraview.h @@ -78,7 +78,7 @@ protected: std::deque> frames; uint32_t draw_frame_id = 0; - int prev_frame_id = 0; + uint32_t prev_frame_id = 0; protected slots: void vipcConnected(VisionIpcClient *vipc_client); diff --git a/system/camerad/cameras/camera_common.cc b/system/camerad/cameras/camera_common.cc index d033d8e6b..3dbe97596 100644 --- a/system/camerad/cameras/camera_common.cc +++ b/system/camerad/cameras/camera_common.cc @@ -159,7 +159,7 @@ void CameraBuf::queue(size_t buf_idx) { // common functions -void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data) { +void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data, CameraState *c) { framed.setFrameId(frame_data.frame_id); framed.setTimestampEof(frame_data.timestamp_eof); framed.setTimestampSof(frame_data.timestamp_sof); @@ -173,6 +173,12 @@ void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &fr framed.setLensErr(frame_data.lens_err); framed.setLensTruePos(frame_data.lens_true_pos); framed.setProcessingTime(frame_data.processing_time); + + if (c->camera_id == CAMERA_ID_AR0231) { + framed.setSensor(cereal::FrameData::ImageSensor::AR0321); + } else if (c->camera_id == CAMERA_ID_OX03C10) { + framed.setSensor(cereal::FrameData::ImageSensor::OX03C10); + } } kj::Array get_raw_frame_image(const CameraBuf *b) { diff --git a/system/camerad/cameras/camera_common.h b/system/camerad/cameras/camera_common.h index 7bbb13c75..bb6de9c8f 100644 --- a/system/camerad/cameras/camera_common.h +++ b/system/camerad/cameras/camera_common.h @@ -113,7 +113,7 @@ public: typedef void (*process_thread_cb)(MultiCameraState *s, CameraState *c, int cnt); -void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data); +void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data, CameraState *c); kj::Array get_raw_frame_image(const CameraBuf *b); float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip); std::thread start_process_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback); diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index b2432bdd7..9bdd71b5d 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -1208,7 +1208,7 @@ static void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) MessageBuilder msg; auto framed = msg.initEvent().initDriverCameraState(); framed.setFrameType(cereal::FrameData::FrameType::FRONT); - fill_frame_data(framed, c->buf.cur_frame_data); + fill_frame_data(framed, c->buf.cur_frame_data, c); if (c->camera_id == CAMERA_ID_AR0231) { ar0231_process_registers(s, c, framed); @@ -1221,7 +1221,7 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) { MessageBuilder msg; auto framed = c == &s->road_cam ? msg.initEvent().initRoadCameraState() : msg.initEvent().initWideRoadCameraState(); - fill_frame_data(framed, b->cur_frame_data); + fill_frame_data(framed, b->cur_frame_data, c); if (env_log_raw_frames && c == &s->road_cam && cnt % 100 == 5) { // no overlap with qlog decimation framed.setImage(get_raw_frame_image(b)); }