diff --git a/opendbc b/opendbc index 526e21da6..b3dc56999 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 526e21da666aeeabcf2369c66903a5675fdf933b +Subproject commit b3dc569994fd10e4de04afd650980c51ddfce5e1 diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index c703ef6cb..16530ed98 100644 --- a/selfdrive/car/chrysler/values.py +++ b/selfdrive/car/chrysler/values.py @@ -241,6 +241,7 @@ FW_VERSIONS = { b'68334977AH', b'68504022AB', b'68530686AB', + b'68504022AC', ], (Ecu.fwdRadar, 0x753, None): [ b'04672895AB', diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index c884f586a..e397f0283 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -220,23 +220,17 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.06]] tire_stiffness_factor = 0.677 - elif candidate == CAR.ODYSSEY: - ret.mass = 4471. * CV.LB_TO_KG + STD_CARGO_KG + elif candidate in (CAR.ODYSSEY, CAR.ODYSSEY_CHN): + ret.mass = 1900. + STD_CARGO_KG ret.wheelbase = 3.00 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 14.35 # as spec - ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end - tire_stiffness_factor = 0.82 - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]] - - elif candidate == CAR.ODYSSEY_CHN: - ret.mass = 1849.2 + STD_CARGO_KG # mean of 4 models in kg - ret.wheelbase = 2.90 - ret.centerToFront = ret.wheelbase * 0.41 # from CAR.ODYSSEY - ret.steerRatio = 14.35 - ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 32767], [0, 32767]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.82 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]] + if candidate == CAR.ODYSSEY_CHN: + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 32767], [0, 32767]] # TODO: determine if there is a dead zone at the top end + else: + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end elif candidate in (CAR.PILOT, CAR.PASSPORT): ret.mass = 4204. * CV.LB_TO_KG + STD_CARGO_KG # average weight diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index e0810e0bb..2510f2e1f 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -1031,6 +1031,23 @@ FW_VERSIONS = { b'54008-THR-A020\x00\x00', ], }, + CAR.ODYSSEY_CHN: { + (Ecu.eps, 0x18da30f1, None): [ + b'39990-T6D-H220\x00\x00', + ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-T6A-J010\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-T6A-F310\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36161-T6A-P040\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-T6A-P110\x00\x00', + ], + }, CAR.PILOT: { (Ecu.shiftByWire, 0x18da0bf1, None): [ b'54008-TG7-A520\x00\x00', diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 913f683e2..2f944edc0 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -49,6 +49,7 @@ class CarController: self.angle_limit_counter = 0 self.frame = 0 + self.accel_last = 0 self.apply_steer_last = 0 self.car_fingerprint = CP.carFingerprint self.last_button_frame = 0 @@ -123,8 +124,9 @@ class CarController: if self.CP.openpilotLongitudinalControl: can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.frame)) if self.frame % 2 == 0: - can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CP, CC.enabled, accel, stopping, CC.cruiseControl.override, + can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CP, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override, set_speed_in_units)) + self.accel_last = accel else: # button presses if (self.frame - self.last_button_frame) * DT_CTRL > 0.25: diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 2e4cb0d25..3cce58186 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -196,10 +196,10 @@ class CarState(CarStateBase): if not self.CP.openpilotLongitudinalControl: speed_factor = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS cp_cruise_info = cp if self.CP.flags & HyundaiFlags.CANFD_HDA2 else cp_cam - ret.cruiseState.speed = cp_cruise_info.vl["CRUISE_INFO"]["SET_SPEED"] * speed_factor - ret.cruiseState.standstill = cp_cruise_info.vl["CRUISE_INFO"]["CRUISE_STANDSTILL"] == 1 - ret.cruiseState.enabled = cp_cruise_info.vl["CRUISE_INFO"]["CRUISE_STATUS"] != 0 - self.cruise_info = copy.copy(cp_cruise_info.vl["CRUISE_INFO"]) + ret.cruiseState.speed = cp_cruise_info.vl["SCC_CONTROL"]["VSetDis"] * speed_factor + ret.cruiseState.standstill = cp_cruise_info.vl["SCC_CONTROL"]["CRUISE_STANDSTILL"] == 1 + ret.cruiseState.enabled = cp_cruise_info.vl["SCC_CONTROL"]["ACCMode"] in (1, 2) + self.cruise_info = copy.copy(cp_cruise_info.vl["SCC_CONTROL"]) cruise_btn_msg = "CRUISE_BUTTONS_ALT" if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else "CRUISE_BUTTONS" self.prev_cruise_buttons = self.cruise_buttons[-1] @@ -464,12 +464,12 @@ class CarState(CarStateBase): if CP.flags & HyundaiFlags.CANFD_HDA2 and not CP.openpilotLongitudinalControl: signals += [ - ("CRUISE_STATUS", "CRUISE_INFO"), - ("SET_SPEED", "CRUISE_INFO"), - ("CRUISE_STANDSTILL", "CRUISE_INFO"), + ("ACCMode", "SCC_CONTROL"), + ("VSetDis", "SCC_CONTROL"), + ("CRUISE_STANDSTILL", "SCC_CONTROL"), ] checks += [ - ("CRUISE_INFO", 50), + ("SCC_CONTROL", 50), ] if CP.carFingerprint in EV_CAR: @@ -497,20 +497,20 @@ class CarState(CarStateBase): checks = [("CAM_0x2a4", 20)] else: signals = [ - ("COUNTER", "CRUISE_INFO"), - ("NEW_SIGNAL_1", "CRUISE_INFO"), - ("CRUISE_MAIN", "CRUISE_INFO"), - ("CRUISE_STATUS", "CRUISE_INFO"), - ("CRUISE_INACTIVE", "CRUISE_INFO"), - ("ZEROS_9", "CRUISE_INFO"), - ("CRUISE_STANDSTILL", "CRUISE_INFO"), - ("ZEROS_5", "CRUISE_INFO"), - ("DISTANCE_SETTING", "CRUISE_INFO"), - ("SET_SPEED", "CRUISE_INFO"), + ("COUNTER", "SCC_CONTROL"), + ("NEW_SIGNAL_1", "SCC_CONTROL"), + ("MainMode_ACC", "SCC_CONTROL"), + ("ACCMode", "SCC_CONTROL"), + ("CRUISE_INACTIVE", "SCC_CONTROL"), + ("ZEROS_9", "SCC_CONTROL"), + ("CRUISE_STANDSTILL", "SCC_CONTROL"), + ("ZEROS_5", "SCC_CONTROL"), + ("DISTANCE_SETTING", "SCC_CONTROL"), + ("VSetDis", "SCC_CONTROL"), ] checks = [ - ("CRUISE_INFO", 50), + ("SCC_CONTROL", 50), ] return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 6) diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index e1478e6f1..8b53e7c37 100644 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -1,3 +1,4 @@ +from common.numpy_fast import clip from selfdrive.car.hyundai.values import HyundaiFlags @@ -52,10 +53,9 @@ def create_buttons(packer, CP, cnt, btn): def create_acc_cancel(packer, CP, cruise_info_copy): values = cruise_info_copy values.update({ - "CRUISE_STATUS": 0, - "CRUISE_INACTIVE": 1, + "ACCMode": 4, }) - return packer.make_can_msg("CRUISE_INFO", get_e_can_bus(CP), values) + return packer.make_can_msg("SCC_CONTROL", get_e_can_bus(CP), values) def create_lfahda_cluster(packer, CP, enabled): values = { @@ -65,33 +65,37 @@ def create_lfahda_cluster(packer, CP, enabled): return packer.make_can_msg("LFAHDA_CLUSTER", get_e_can_bus(CP), values) -def create_acc_control(packer, CP, enabled, accel, stopping, gas_override, set_speed): - cruise_status = 0 if not enabled else (4 if gas_override else 2) +def create_acc_control(packer, CP, enabled, accel_last, accel, stopping, gas_override, set_speed): + jerk = 5 + jn = jerk / 50 if not enabled or gas_override: - accel = 0 + a_val, a_raw = 0, 0 + else: + a_raw = accel + a_val = clip(accel, accel_last - jn, accel_last + jn) + if stopping: + a_raw = 0 + values = { - "CRUISE_STATUS": cruise_status, - "CRUISE_INACTIVE": 0 if enabled else 1, - "CRUISE_MAIN": 1, - "CRUISE_STANDSTILL": 0, - "STOP_REQ": 1 if stopping else 0, - "ACCEL_REQ": accel, - "ACCEL_REQ2": accel, - "SET_SPEED": set_speed, - "DISTANCE_SETTING": 4, + "ACCMode": 0 if not enabled else (2 if gas_override else 1), + "MainMode_ACC": 1, + "StopReq": 1 if stopping else 0, + "aReqValue": a_val, + "aReqRaw": a_raw, + "VSetDis": set_speed, + "JerkLowerLimit": jerk if enabled else 1, "ACC_ObjDist": 1, - "ObjValid": 1, + "ObjValid": 0, "OBJ_STATUS": 2, - "SET_ME_2": 0x2, + "SET_ME_2": 0x4, "SET_ME_3": 0x3, "SET_ME_TMP_64": 0x64, - - "NEW_SIGNAL_9": 2, "NEW_SIGNAL_10": 4, + "DISTANCE_SETTING": 4, } - return packer.make_can_msg("CRUISE_INFO", get_e_can_bus(CP), values) + return packer.make_can_msg("SCC_CONTROL", get_e_can_bus(CP), values) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 6a1d741de..4b4d51f3f 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -202,14 +202,10 @@ class CarInterface(CarInterfaceBase): if candidate in CANFD_CAR: ret.longitudinalTuning.kpV = [0.1] ret.longitudinalTuning.kiV = [0.0] - ret.longitudinalActuatorDelayLowerBound = 0.15 - ret.longitudinalActuatorDelayUpperBound = 0.5 ret.experimentalLongitudinalAvailable = bool(ret.flags & HyundaiFlags.CANFD_HDA2) else: ret.longitudinalTuning.kpV = [0.5] ret.longitudinalTuning.kiV = [0.0] - ret.longitudinalActuatorDelayLowerBound = 0.5 - ret.longitudinalActuatorDelayUpperBound = 0.5 ret.experimentalLongitudinalAvailable = candidate not in (LEGACY_SAFETY_MODE_CAR | CAMERA_SCC_CAR) ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable ret.pcmCruise = not ret.openpilotLongitudinalControl @@ -218,6 +214,8 @@ class CarInterface(CarInterfaceBase): ret.startingState = True ret.vEgoStarting = 0.1 ret.startAccel = 2.0 + ret.longitudinalActuatorDelayLowerBound = 0.5 + ret.longitudinalActuatorDelayUpperBound = 0.5 # *** feature detection *** if candidate in CANFD_CAR: diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 98cb72293..c760c724a 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -1232,6 +1232,7 @@ FW_VERSIONS = { b'\xf1\x87\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x00CN7 MDPS C 1.00 1.06 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4CNDC106', b'\xf1\x8756310/AA070\xf1\x00CN7 MDPS C 1.00 1.06 56310/AA070 4CNDC106', b'\xf1\x8756310AA050\x00\xf1\x00CN7 MDPS C 1.00 1.06 56310AA050\x00 4CNDC106', + b'\xf1\x8756310AA050\x00\xf1\x00CN7 MDPS C 1.00 1.06 56310AA050\x00 4CNDC106\xf1\xa01.06', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.00 99210-AB000 200819', @@ -1244,6 +1245,7 @@ FW_VERSIONS = { b'\xf1\x8758910-AA800\xf1\x00CN ESC \t 104 \x08\x03 58910-AA800', b'\xf1\x8758910-AB800\xf1\x00CN ESC \t 101 \x10\x03 58910-AB800', b'\xf1\x8758910-AA800\xf1\x00CN ESC \t 105 \x10\x03 58910-AA800', + b'\xf1\x8758910-AB800\xf1\x00CN ESC \t 101 \x10\x03 58910-AB800\xf1\xa01.01', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x00HT6WA280BLHT6VA640A1CCN0N20NS5\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py index a5ca23711..d6b6d17d5 100644 --- a/selfdrive/car/nissan/carstate.py +++ b/selfdrive/car/nissan/carstate.py @@ -44,7 +44,7 @@ class CarState(CarStateBase): ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4. ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) - ret.standstill = ret.vEgoRaw < 0.01 + ret.standstill = cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RL"] == 0.0 and cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RR"] == 0.0 if self.CP.carFingerprint == CAR.ALTIMA: ret.cruiseState.enabled = bool(cp.vl["CRUISE_STATE"]["CRUISE_ENABLED"]) diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index 5838c315a..d83320342 100644 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -25,6 +25,7 @@ non_tested_cars = [ GM.BOLT_EV, HYUNDAI.GENESIS_G90, HYUNDAI.KIA_OPTIMA_H, + HONDA.ODYSSEY_CHN, ] CarTestRoute = namedtuple('CarTestRoute', ['route', 'car_model', 'segment'], defaults=(None,)) @@ -61,7 +62,6 @@ routes = [ CarTestRoute("2c4292a5cd10536c|2021-08-19--21-32-15", HONDA.FREED), CarTestRoute("03be5f2fd5c508d1|2020-04-19--18-44-15", HONDA.HRV), CarTestRoute("917b074700869333|2021-05-24--20-40-20", HONDA.ACURA_ILX), - CarTestRoute("81722949a62ea724|2019-04-06--15-19-25", HONDA.ODYSSEY_CHN), CarTestRoute("08a3deb07573f157|2020-03-06--16-11-19", HONDA.ACCORD), # 1.5T CarTestRoute("1da5847ac2488106|2021-05-24--19-31-50", HONDA.ACCORD), # 2.0T CarTestRoute("085ac1d942c35910|2021-03-25--20-11-15", HONDA.ACCORD), # 2021 with new style HUD msgs diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index e4e141153..ee25b8205 100755 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -9,7 +9,7 @@ from parameterized import parameterized_class from cereal import log, car from common.realtime import DT_CTRL -from selfdrive.boardd.boardd import can_capnp_to_can_list, can_list_to_can_capnp +from selfdrive.boardd.boardd import can_capnp_to_can_list from selfdrive.car.fingerprints import all_known_cars from selfdrive.car.car_helpers import interfaces from selfdrive.car.gm.values import CAR as GM @@ -109,6 +109,10 @@ class TestCarModelBase(unittest.TestCase): assert cls.CP assert cls.CP.carFingerprint == cls.car_model + @classmethod + def tearDownClass(cls): + del cls.can_msgs + def setUp(self): self.CI = self.CarInterface(self.CP, self.CarController, self.CarState) assert self.CI @@ -210,18 +214,15 @@ class TestCarModelBase(unittest.TestCase): # warm up pass, as initial states may be different for can in self.can_msgs[:300]: + self.CI.update(CC, (can.as_builder().to_bytes(), )) for msg in can_capnp_to_can_list(can.can, src_filter=range(64)): to_send = package_can_msg(msg) self.safety.safety_rx_hook(to_send) - self.CI.update(CC, (can_list_to_can_capnp([msg, ]), )) - - if not self.CP.pcmCruise: - self.safety.set_controls_allowed(0) controls_allowed_prev = False CS_prev = car.CarState.new_message() checks = defaultdict(lambda: 0) - for can in self.can_msgs: + for idx, can in enumerate(self.can_msgs): CS = self.CI.update(CC, (can.as_builder().to_bytes(), )) for msg in can_capnp_to_can_list(can.can, src_filter=range(64)): msg = list(msg) @@ -230,10 +231,17 @@ class TestCarModelBase(unittest.TestCase): ret = self.safety.safety_rx_hook(to_send) self.assertEqual(1, ret, f"safety rx failed ({ret=}): {to_send}") + # Skip first frame so CS_prev is properly initialized + if idx == 0: + CS_prev = CS + # Button may be left pressed in warm up period + if not self.CP.pcmCruise: + self.safety.set_controls_allowed(0) + continue + # TODO: check rest of panda's carstate (steering, ACC main on, etc.) checks['gasPressed'] += CS.gasPressed != self.safety.get_gas_pressed_prev() - checks['cruiseState'] += CS.cruiseState.enabled and not CS.cruiseState.available if self.CP.carName not in ("hyundai", "volkswagen", "body"): # TODO: fix standstill mismatches for other makes checks['standstill'] += CS.standstill == self.safety.get_vehicle_moving() diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index c7d81c349..0e4ac9478 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -e56c5a6ac5b87ee6083c9f92921e7198591f7b5d +fc3a044c567a8702ed1500d745170c365dd6b3d4 \ No newline at end of file diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 346cb3ab9..d3bc931b6 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -227,14 +227,6 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { setProperty("dmActive", sm["driverMonitoringState"].getDriverMonitoringState().getIsActiveMode()); setProperty("rightHandDM", sm["driverMonitoringState"].getDriverMonitoringState().getIsRHD()); } - - setStreamType(s.scene.wide_cam ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD); - if (s.scene.calibration_valid) { - auto calib = s.scene.wide_cam ? s.scene.view_from_wide_calib : s.scene.view_from_calib; - CameraWidget::updateCalibration(calib); - } else { - CameraWidget::updateCalibration(DEFAULT_CALIBRATION); - } } void AnnotatedCameraWidget::drawHud(QPainter &p) { @@ -545,18 +537,62 @@ void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::ModelDataV } void AnnotatedCameraWidget::paintGL() { + UIState *s = uiState(); + SubMaster &sm = *(s->sm); const double start_draw_t = millis_since_boot(); + const cereal::ModelDataV2::Reader &model = sm["modelV2"].getModelV2(); - UIState *s = uiState(); - const cereal::ModelDataV2::Reader &model = (*s->sm)["modelV2"].getModelV2(); - CameraWidget::setFrameId(model.getFrameId()); - CameraWidget::paintGL(); + // draw camera frame + { + std::lock_guard lk(frame_lock); + + if (frames.empty()) { + if (skip_frame_count > 0) { + skip_frame_count--; + qDebug() << "skipping frame, not ready"; + return; + } + } else { + // skip drawing up to this many frames if we're + // missing camera frames. this smooths out the + // transitions from the narrow and wide cameras + skip_frame_count = 5; + } + + // Wide or narrow cam dependent on speed + float v_ego = sm["carState"].getCarState().getVEgo(); + if ((v_ego < 10) || s->wide_cam_only) { + wide_cam_requested = true; + } else if (v_ego > 15) { + wide_cam_requested = false; + } + // TODO: also detect when ecam vision stream isn't available + // for replay of old routes, never go to widecam + wide_cam_requested = wide_cam_requested && s->scene.calibration_wide_valid; + CameraWidget::setStreamType(wide_cam_requested ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD); + + s->scene.wide_cam = CameraWidget::getStreamType() == VISION_STREAM_WIDE_ROAD; + if (s->scene.calibration_valid) { + auto calib = s->scene.wide_cam ? s->scene.view_from_wide_calib : s->scene.view_from_calib; + CameraWidget::updateCalibration(calib); + } else { + CameraWidget::updateCalibration(DEFAULT_CALIBRATION); + } + CameraWidget::setFrameId(model.getFrameId()); + CameraWidget::paintGL(); + } QPainter painter(this); painter.setRenderHint(QPainter::Antialiasing); painter.setPen(Qt::NoPen); if (s->worldObjectsVisible()) { + if (sm.rcv_frame("modelV2") > s->scene.started_frame) { + update_model(s, sm["modelV2"].getModelV2()); + if (sm.rcv_frame("radarState") > s->scene.started_frame) { + update_leads(s, sm["radarState"].getRadarState(), sm["modelV2"].getModelV2().getPosition()); + } + } drawLaneLines(painter, s); diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 2a663185f..7edca6b3d 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -70,6 +70,9 @@ private: int status = STATUS_DISENGAGED; std::unique_ptr pm; + int skip_frame_count = 0; + bool wide_cam_requested = false; + protected: void paintGL() override; void initializeGL() override; diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index d7591633c..a606d6893 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -94,7 +94,7 @@ mat4 get_fit_view_transform(float widget_aspect_ratio, float frame_aspect_ratio) } // namespace CameraWidget::CameraWidget(std::string stream_name, VisionStreamType type, bool zoom, QWidget* parent) : - stream_name(stream_name), stream_type(type), zoomed_view(zoom), QOpenGLWidget(parent) { + stream_name(stream_name), requested_stream_type(type), zoomed_view(zoom), QOpenGLWidget(parent) { setAttribute(Qt::WA_OpaquePaintEvent); QObject::connect(this, &CameraWidget::vipcThreadConnected, this, &CameraWidget::vipcConnected, Qt::BlockingQueuedConnection); QObject::connect(this, &CameraWidget::vipcThreadFrameReceived, this, &CameraWidget::vipcFrameReceived, Qt::QueuedConnection); @@ -124,7 +124,7 @@ void CameraWidget::initializeGL() { GLint frame_pos_loc = program->attributeLocation("aPosition"); GLint frame_texcoord_loc = program->attributeLocation("aTexCoord"); - auto [x1, x2, y1, y2] = stream_type == VISION_STREAM_DRIVER ? std::tuple(0.f, 1.f, 1.f, 0.f) : std::tuple(1.f, 0.f, 1.f, 0.f); + auto [x1, x2, y1, y2] = requested_stream_type == VISION_STREAM_DRIVER ? std::tuple(0.f, 1.f, 1.f, 0.f) : std::tuple(1.f, 0.f, 1.f, 0.f); const uint8_t frame_indicies[] = {0, 1, 2, 0, 2, 3}; const float frame_coords[4][4] = { {-1.0, -1.0, x2, y1}, // bl @@ -163,36 +163,26 @@ void CameraWidget::initializeGL() { void CameraWidget::showEvent(QShowEvent *event) { if (!vipc_thread) { + clearFrames(); vipc_thread = new QThread(); connect(vipc_thread, &QThread::started, [=]() { vipcThread(); }); connect(vipc_thread, &QThread::finished, vipc_thread, &QObject::deleteLater); vipc_thread->start(); } - clearFrames(); -} - -void CameraWidget::hideEvent(QHideEvent *event) { - if (vipc_thread) { - vipc_thread->requestInterruption(); - vipc_thread->quit(); - vipc_thread->wait(); - vipc_thread = nullptr; - } - clearFrames(); } void CameraWidget::updateFrameMat() { int w = width(), h = height(); if (zoomed_view) { - if (stream_type == VISION_STREAM_DRIVER) { + if (active_stream_type == VISION_STREAM_DRIVER) { frame_mat = get_driver_view_transform(w, h, stream_width, stream_height); } else { // Project point at "infinity" to compute x and y offsets // to ensure this ends up in the middle of the screen // for narrow come and a little lower for wide cam. // TODO: use proper perspective transform? - if (stream_type == VISION_STREAM_WIDE_ROAD) { + if (active_stream_type == VISION_STREAM_WIDE_ROAD) { intrinsic_matrix = ecam_intrinsic_matrix; zoom = 2.0; } else { @@ -212,11 +202,11 @@ void CameraWidget::updateFrameMat() { x_offset = std::clamp(x_offset_, -max_x_offset, max_x_offset); y_offset = std::clamp(y_offset_, -max_y_offset, max_y_offset); - float zx = zoom * 2 * intrinsic_matrix.v[2] / width(); - float zy = zoom * 2 * intrinsic_matrix.v[5] / height(); + float zx = zoom * 2 * intrinsic_matrix.v[2] / w; + float zy = zoom * 2 * intrinsic_matrix.v[5] / h; const mat4 frame_transform = {{ - zx, 0.0, 0.0, -x_offset / width() * 2, - 0.0, zy, 0.0, y_offset / height() * 2, + zx, 0.0, 0.0, -x_offset / w * 2, + 0.0, zy, 0.0, y_offset / h * 2, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, }}; @@ -224,7 +214,7 @@ void CameraWidget::updateFrameMat() { } } else if (stream_width > 0 && stream_height > 0) { // fit frame to widget size - float widget_aspect_ratio = (float)width() / height(); + float widget_aspect_ratio = (float)w / h; float frame_aspect_ratio = (float)stream_width / stream_height; frame_mat = get_fit_view_transform(widget_aspect_ratio, frame_aspect_ratio); } @@ -232,7 +222,6 @@ void CameraWidget::updateFrameMat() { void CameraWidget::updateCalibration(const mat3 &calib) { calibration = calib; - updateFrameMat(); } void CameraWidget::paintGL() { @@ -240,13 +229,26 @@ void CameraWidget::paintGL() { glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); std::lock_guard lk(frame_lock); + if (frames.empty()) return; + + int frame_idx = frames.size() - 1; + + // Always draw latest frame until sync logic is more stable + // for (frame_idx = 0; frame_idx < frames.size() - 1; frame_idx++) { + // if (frames[frame_idx].first == draw_frame_id) break; + // } - // use previous texture if update() is called without new frame. - VisionBuf *frame = nullptr; - if (!frames.empty()) { - frame = frames.front().second; - frames.pop_front(); + // Log duplicate/dropped frames + if (frames[frame_idx].first == prev_frame_id) { + qDebug() << "Drawing same frame twice" << frames[frame_idx].first; + } else if (frames[frame_idx].first != prev_frame_id + 1) { + qDebug() << "Skipped frame" << frames[frame_idx].first; } + prev_frame_id = frames[frame_idx].first; + VisionBuf *frame = frames[frame_idx].second; + assert(frame != nullptr); + + updateFrameMat(); glViewport(0, 0, width(), height()); glBindVertexArray(frame_vao); @@ -254,22 +256,22 @@ void CameraWidget::paintGL() { glPixelStorei(GL_UNPACK_ALIGNMENT, 1); #ifdef QCOM2 + // no frame copy glActiveTexture(GL_TEXTURE0); - if (frame) { - glEGLImageTargetTexture2DOES(GL_TEXTURE_EXTERNAL_OES, egl_images[frame->idx]); - assert(glGetError() == GL_NO_ERROR); - } + glEGLImageTargetTexture2DOES(GL_TEXTURE_EXTERNAL_OES, egl_images[frame->idx]); + assert(glGetError() == GL_NO_ERROR); #else + // fallback to copy glPixelStorei(GL_UNPACK_ROW_LENGTH, stream_stride); glActiveTexture(GL_TEXTURE0); glBindTexture(GL_TEXTURE_2D, textures[0]); - if (frame) glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, stream_width, stream_height, GL_RED, GL_UNSIGNED_BYTE, frame->y); + glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, stream_width, stream_height, GL_RED, GL_UNSIGNED_BYTE, frame->y); assert(glGetError() == GL_NO_ERROR); glPixelStorei(GL_UNPACK_ROW_LENGTH, stream_stride/2); glActiveTexture(GL_TEXTURE0 + 1); glBindTexture(GL_TEXTURE_2D, textures[1]); - if (frame) glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, stream_width/2, stream_height/2, GL_RG, GL_UNSIGNED_BYTE, frame->uv); + glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, stream_width/2, stream_height/2, GL_RG, GL_UNSIGNED_BYTE, frame->uv); assert(glGetError() == GL_NO_ERROR); #endif @@ -332,8 +334,6 @@ void CameraWidget::vipcConnected(VisionIpcClient *vipc_client) { glTexImage2D(GL_TEXTURE_2D, 0, GL_RG8, stream_width/2, stream_height/2, 0, GL_RG, GL_UNSIGNED_BYTE, nullptr); assert(glGetError() == GL_NO_ERROR); #endif - - updateFrameMat(); } void CameraWidget::vipcFrameReceived() { @@ -341,16 +341,18 @@ void CameraWidget::vipcFrameReceived() { } void CameraWidget::vipcThread() { - VisionStreamType cur_stream_type = stream_type; + VisionStreamType cur_stream = requested_stream_type; std::unique_ptr vipc_client; VisionIpcBufExtra meta_main = {0}; while (!QThread::currentThread()->isInterruptionRequested()) { - if (!vipc_client || cur_stream_type != stream_type) { + if (!vipc_client || cur_stream != requested_stream_type) { clearFrames(); - cur_stream_type = stream_type; - vipc_client.reset(new VisionIpcClient(stream_name, cur_stream_type, false)); + qDebug() << "connecting to stream " << requested_stream_type << ", was connected to " << cur_stream; + cur_stream = requested_stream_type;; + vipc_client.reset(new VisionIpcClient(stream_name, cur_stream, false)); } + active_stream_type = cur_stream; if (!vipc_client->connected) { clearFrames(); @@ -366,7 +368,6 @@ void CameraWidget::vipcThread() { std::lock_guard lk(frame_lock); frames.push_back(std::make_pair(meta_main.frame_id, buf)); while (frames.size() > FRAME_BUFFER_SIZE) { - qDebug() << "Skipped frame" << frames.front().first; frames.pop_front(); } } diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h index aff3abc83..0698d1fb9 100644 --- a/selfdrive/ui/qt/widgets/cameraview.h +++ b/selfdrive/ui/qt/widgets/cameraview.h @@ -31,9 +31,10 @@ public: using QOpenGLWidget::QOpenGLWidget; explicit CameraWidget(std::string stream_name, VisionStreamType stream_type, bool zoom, QWidget* parent = nullptr); ~CameraWidget(); - void setStreamType(VisionStreamType type) { stream_type = type; } void setBackgroundColor(const QColor &color) { bg = color; } void setFrameId(int frame_id) { draw_frame_id = frame_id; } + void setStreamType(VisionStreamType type) { requested_stream_type = type; } + VisionStreamType getStreamType() { return active_stream_type; } signals: void clicked(); @@ -45,7 +46,6 @@ protected: void initializeGL() override; void resizeGL(int w, int h) override { updateFrameMat(); } void showEvent(QShowEvent *event) override; - void hideEvent(QHideEvent *event) override; void mouseReleaseEvent(QMouseEvent *event) override { emit clicked(); } virtual void updateFrameMat(); void updateCalibration(const mat3 &calib); @@ -68,7 +68,8 @@ protected: int stream_width = 0; int stream_height = 0; int stream_stride = 0; - std::atomic stream_type; + std::atomic active_stream_type; + std::atomic requested_stream_type; QThread *vipc_thread = nullptr; // Calibration @@ -78,9 +79,10 @@ protected: mat3 calibration = DEFAULT_CALIBRATION; mat3 intrinsic_matrix = fcam_intrinsic_matrix; - std::mutex frame_lock; + std::recursive_mutex frame_lock; std::deque> frames; uint32_t draw_frame_id = 0; + uint32_t prev_frame_id = 0; protected slots: void vipcConnected(VisionIpcClient *vipc_client); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index d1d72b4da..821064f81 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -35,7 +35,7 @@ static bool calib_frame_to_full_frame(const UIState *s, float in_x, float in_y, return false; } -static int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line, const float path_height) { +int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line, const float path_height) { const auto line_x = line.getX(); int max_idx = 0; for (int i = 1; i < TRAJECTORY_SIZE && line_x[i] <= path_height; ++i) { @@ -44,7 +44,7 @@ static int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line return max_idx; } -static void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, const cereal::ModelDataV2::XYZTData::Reader &line) { +void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, const cereal::ModelDataV2::XYZTData::Reader &line) { for (int i = 0; i < 2; ++i) { auto lead_data = (i == 0) ? radar_state.getLeadOne() : radar_state.getLeadTwo(); if (lead_data.getStatus()) { @@ -54,7 +54,7 @@ static void update_leads(UIState *s, const cereal::RadarState::Reader &radar_sta } } -static void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, +void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, float y_off, float z_off, QPolygonF *pvd, int max_idx, bool allow_invert=true) { const auto line_x = line.getX(), line_y = line.getY(), line_z = line.getZ(); @@ -78,7 +78,7 @@ static void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTDa *pvd = left_points + right_points; } -static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) { +void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) { UIScene &scene = s->scene; auto model_position = model.getPosition(); float max_distance = std::clamp(model_position.getX()[TRAJECTORY_SIZE - 1], @@ -141,14 +141,7 @@ static void update_state(UIState *s) { } } scene.calibration_valid = sm["liveCalibration"].getLiveCalibration().getCalStatus() == 1; - } - if (s->worldObjectsVisible()) { - if (sm.updated("modelV2")) { - update_model(s, sm["modelV2"].getModelV2()); - } - if (sm.updated("radarState") && sm.rcv_frame("modelV2") > s->scene.started_frame) { - update_leads(s, sm["radarState"].getRadarState(), sm["modelV2"].getModelV2().getPosition()); - } + scene.calibration_wide_valid = wfde_list.size() == 3; } if (sm.updated("pandaStates")) { auto pandaStates = sm["pandaStates"].getPandaStates(); @@ -173,17 +166,6 @@ static void update_state(UIState *s) { scene.light_sensor = std::max(100.0f - scale * sm["wideRoadCameraState"].getWideRoadCameraState().getExposureValPercent(), 0.0f); } scene.started = sm["deviceState"].getDeviceState().getStarted() && scene.ignition; - - if (sm.updated("carState")) { - float v_ego = sm["carState"].getCarState().getVEgo(); - // TODO: support replays without ecam by using fcam - // Wide or narrow cam dependent on speed - if ((v_ego < 10) || s->wide_cam_only) { - scene.wide_cam = true; - } else if (v_ego > 15) { - scene.wide_cam = false; - } - } } void ui_update_params(UIState *s) { diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 5a51dda8f..e550afd5f 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -87,6 +87,7 @@ const QColor bg_colors [] = { typedef struct UIScene { bool calibration_valid = false; + bool calibration_wide_valid = false; bool wide_cam = true; mat3 view_from_calib = DEFAULT_CALIBRATION; mat3 view_from_wide_calib = DEFAULT_CALIBRATION; @@ -181,3 +182,8 @@ public slots: }; void ui_update_params(UIState *s); +int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line, const float path_height); +void update_model(UIState *s, const cereal::ModelDataV2::Reader &model); +void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, const cereal::ModelDataV2::XYZTData::Reader &line); +void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, + float y_off, float z_off, QPolygonF *pvd, int max_idx, bool allow_invert); diff --git a/tools/cabana/chartswidget.cc b/tools/cabana/chartswidget.cc index 21fd71b8e..56346c6d6 100644 --- a/tools/cabana/chartswidget.cc +++ b/tools/cabana/chartswidget.cc @@ -402,10 +402,13 @@ void ChartView::mouseReleaseEvent(QMouseEvent *event) { // zoom in if selected range is greater than 0.5s emit zoomIn(min, max); } + event->accept(); } else if (event->button() == Qt::RightButton) { emit zoomReset(); + event->accept(); + } else { + QGraphicsView::mouseReleaseEvent(event); } - event->accept(); } void ChartView::mouseMoveEvent(QMouseEvent *ev) { diff --git a/tools/cabana/dbcmanager.cc b/tools/cabana/dbcmanager.cc index 479b14afe..0ab67dd30 100644 --- a/tools/cabana/dbcmanager.cc +++ b/tools/cabana/dbcmanager.cc @@ -1,5 +1,6 @@ #include "tools/cabana/dbcmanager.h" +#include #include #include @@ -41,8 +42,8 @@ QString DBCManager::generateDBC() { .arg(sig.size) .arg(sig.is_little_endian ? '1' : '0') .arg(sig.is_signed ? '-' : '+') - .arg(sig.factor, 0, 'g', 20) - .arg(sig.offset); + .arg(sig.factor, 0, 'g', std::numeric_limits::digits10) + .arg(sig.offset, 0, 'g', std::numeric_limits::digits10); } dbc_string += "\n"; } diff --git a/tools/cabana/messageswidget.cc b/tools/cabana/messageswidget.cc index 4d97bba58..205e5347c 100644 --- a/tools/cabana/messageswidget.cc +++ b/tools/cabana/messageswidget.cc @@ -98,6 +98,7 @@ void MessagesWidget::loadDBCFromPaste() { if (dlg.exec()) { dbc()->open("from paste", dlg.dbc_edit->toPlainText()); dbc_combo->setCurrentText("loaded from paste"); + model->updateState(true); } } diff --git a/tools/plotjuggler/juggle.py b/tools/plotjuggler/juggle.py index b7c32d9bf..1e592da3b 100755 --- a/tools/plotjuggler/juggle.py +++ b/tools/plotjuggler/juggle.py @@ -89,7 +89,7 @@ def juggle_route(route_or_segment_name, segment_count, qlog, can, layout, dbc=No query = parse_qs(urlparse(route_or_segment_name).query) route_or_segment_name = query["route"][0] - if route_or_segment_name.startswith(("http://", "https://")) or os.path.isfile(route_or_segment_name): + if route_or_segment_name.startswith(("http://", "https://", "cd:/")) or os.path.isfile(route_or_segment_name): logs = [route_or_segment_name] elif ci: route_or_segment_name = SegmentName(route_or_segment_name, allow_route_name=True) diff --git a/tools/sim/start_openpilot_docker.sh b/tools/sim/start_openpilot_docker.sh index e48e63574..dba89b8b7 100755 --- a/tools/sim/start_openpilot_docker.sh +++ b/tools/sim/start_openpilot_docker.sh @@ -20,6 +20,7 @@ else EXTRA_ARGS="${EXTRA_ARGS} -it" fi +docker kill openpilot_client || true docker run --net=host\ --name openpilot_client \ --rm \