From 1c3446a0dc5927a64000bc5610547cb485f569c3 Mon Sep 17 00:00:00 2001 From: ctidder2 <32351779+ctidder2@users.noreply.github.com> Date: Fri, 27 May 2022 15:02:20 -0700 Subject: [PATCH 01/29] Toyota: Add missing Highlander Hybrid TSS2 fwdRadar and fwdCamera (#24664) --- selfdrive/car/toyota/values.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index d42db26b91..b170592744 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -963,10 +963,12 @@ FW_VERSIONS = { (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301400\x00\x00\x00\x00', b'\x018821F6201200\x00\x00\x00\x00', + b'\x018821F6201300\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ b'\x028646F0E02100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', b'\x028646F4803000\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', + b'\x028646F4803000\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', ], }, CAR.LEXUS_IS: { From 2c2e0ae8a770e2cb648111624a86f0e307d52bb3 Mon Sep 17 00:00:00 2001 From: BoBowchan Date: Fri, 27 May 2022 15:04:06 -0700 Subject: [PATCH 02/29] HKG: add missing fwdRadar for Kia Niro EV 2022 (#24665) Co-authored-by: Bohan Li --- selfdrive/car/hyundai/values.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 396bfaa924..76d9aa4f93 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -946,6 +946,7 @@ FW_VERSIONS = { b'\xf1\x8799110Q4000\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4000 ', b'\xf1\x8799110Q4100\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4100 ', b'\xf1\x8799110Q4500\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4500 ', + b'\xf1\x8799110Q4600\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4600 ', b'\xf1\x8799110Q4600\xf1\x00DEev SCC FNCUP 1.00 1.00 99110-Q4600 ', b'\xf1\x8799110Q4600\xf1\x00DEev SCC FHCUP 1.00 1.00 99110-Q4600 ', ], From 7115f1f7c8e43a4e97fcb7bfb51f84b45eaedc6f Mon Sep 17 00:00:00 2001 From: ntegan1 Date: Fri, 27 May 2022 18:11:16 -0400 Subject: [PATCH 03/29] TSS-P Corolla: use torque lateral control (#24291) use torque controller for tssp corolla Co-authored-by: ntegan --- selfdrive/car/toyota/interface.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 3d59102e43..1b577cc6b7 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -61,7 +61,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 18.27 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - set_lat_tune(ret.lateralTuning, LatTunes.PID_A) + set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=2.8, FRICTION=0.024) elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RXH, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2): stop_and_go = True From fb4b4773a58a62783dd48e6058be5a0da6bb1fab Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 27 May 2022 15:35:09 -0700 Subject: [PATCH 04/29] UI: draw map with uiUpdate signal (#24632) * draw map with same uiUpdate signal as onroad window * fix * fix * fix * test * test * clean up * clean up * clean up * draw instantly when dragging * self explanatory * remove line * fix spacing * see if we call ui'c sm->update before mapwindow * Revert "see if we call ui'c sm->update before mapwindow" This reverts commit e5dcd70013750b871894c276e4d15d55b0c92e97. * sort includes --- selfdrive/ui/qt/maps/map.cc | 41 +++++++++++++++---------------------- selfdrive/ui/qt/maps/map.h | 16 +++++++-------- selfdrive/ui/ui.cc | 2 +- 3 files changed, 24 insertions(+), 35 deletions(-) diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index 8e5a02f8a0..4c6a0a4e65 100644 --- a/selfdrive/ui/qt/maps/map.cc +++ b/selfdrive/ui/qt/maps/map.cc @@ -3,14 +3,14 @@ #include #include -#include #include +#include #include "common/swaglog.h" -#include "selfdrive/ui/ui.h" -#include "selfdrive/ui/qt/util.h" #include "selfdrive/ui/qt/maps/map_helpers.h" #include "selfdrive/ui/qt/request_repeater.h" +#include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/ui.h" const int PAN_TIMEOUT = 100; @@ -24,16 +24,8 @@ const float MAP_SCALE = 2; const QString ICON_SUFFIX = ".png"; -MapWindow::MapWindow(const QMapboxGLSettings &settings) : - m_settings(settings), velocity_filter(0, 10, 0.05) { - sm = new SubMaster({"liveLocationKalman", "navInstruction", "navRoute"}); - - // Connect now, so any navRoutes sent while the map is initializing are not dropped - sm->update(0); - - timer = new QTimer(this); - QObject::connect(timer, SIGNAL(timeout()), this, SLOT(timerUpdate())); - timer->start(50); +MapWindow::MapWindow(const QMapboxGLSettings &settings) : m_settings(settings), velocity_filter(0, 10, 0.05) { + QObject::connect(uiState(), &UIState::uiUpdate, this, &MapWindow::updateState); // Instructions map_instructions = new MapInstructions(this); @@ -105,16 +97,15 @@ void MapWindow::initLayers() { } } -void MapWindow::timerUpdate() { +void MapWindow::updateState(const UIState &s) { if (!uiState()->scene.started) { return; } - + const SubMaster &sm = *(s.sm); update(); - sm->update(0); - if (sm->updated("liveLocationKalman")) { - auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman(); + if (sm.updated("liveLocationKalman")) { + auto location = sm["liveLocationKalman"].getLiveLocationKalman(); auto pos = location.getPositionGeodetic(); auto orientation = location.getCalibratedOrientationNED(); auto velocity = location.getVelocityCalibrated(); @@ -129,7 +120,7 @@ void MapWindow::timerUpdate() { } } - if (sm->updated("navRoute") && (*sm)["navRoute"].getNavRoute().getCoordinates().size()) { + if (sm.updated("navRoute") && sm["navRoute"].getNavRoute().getCoordinates().size()) { qWarning() << "Got new navRoute from navd. Opening map:" << allow_open; // Only open the map on setting destination the first time @@ -178,9 +169,9 @@ void MapWindow::timerUpdate() { zoom_counter--; } - if (sm->updated("navInstruction")) { - if (sm->valid("navInstruction")) { - auto i = (*sm)["navInstruction"].getNavInstruction(); + if (sm.updated("navInstruction")) { + if (sm.valid("navInstruction")) { + auto i = sm["navInstruction"].getNavInstruction(); emit ETAChanged(i.getTimeRemaining(), i.getTimeRemainingTypical(), i.getDistanceRemaining()); if (localizer_valid) { @@ -194,9 +185,9 @@ void MapWindow::timerUpdate() { } } - if (sm->rcv_frame("navRoute") != route_rcv_frame) { + if (sm.rcv_frame("navRoute") != route_rcv_frame) { qWarning() << "Updating navLayer with new route"; - auto route = (*sm)["navRoute"].getNavRoute(); + auto route = sm["navRoute"].getNavRoute(); auto route_points = capnp_coordinate_list_to_collection(route.getCoordinates()); QMapbox::Feature feature(QMapbox::Feature::LineStringType, route_points, {}, {}); QVariantMap navSource; @@ -205,7 +196,7 @@ void MapWindow::timerUpdate() { m_map->updateSource("navSource", navSource); m_map->setLayoutProperty("navLayer", "visibility", "visible"); - route_rcv_frame = sm->rcv_frame("navRoute"); + route_rcv_frame = sm.rcv_frame("navRoute"); } } diff --git a/selfdrive/ui/qt/maps/map.h b/selfdrive/ui/qt/maps/map.h index 01a13f3b7c..7c39b24c3c 100644 --- a/selfdrive/ui/qt/maps/map.h +++ b/selfdrive/ui/qt/maps/map.h @@ -5,22 +5,22 @@ #include #include #include -#include #include +#include #include #include #include +#include #include #include -#include -#include +#include #include -#include -#include +#include +#include "cereal/messaging/messaging.h" #include "common/params.h" #include "common/util.h" -#include "cereal/messaging/messaging.h" +#include "selfdrive/ui/ui.h" class MapInstructions : public QWidget { Q_OBJECT @@ -91,8 +91,6 @@ private: void pinchTriggered(QPinchGesture *gesture); bool m_sourceAdded = false; - SubMaster *sm; - QTimer* timer; bool loaded_once = false; bool allow_open = true; @@ -115,7 +113,7 @@ private: uint64_t route_rcv_frame = 0; private slots: - void timerUpdate(); + void updateState(const UIState &s); public slots: void offroadTransition(bool offroad); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 985b0259d2..b31e84905d 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -233,7 +233,7 @@ UIState::UIState(QObject *parent) : QObject(parent) { sm = std::make_unique>({ "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "roadCameraState", "pandaStates", "carParams", "driverMonitoringState", "sensorEvents", "carState", "liveLocationKalman", - "wideRoadCameraState", "managerState", + "wideRoadCameraState", "managerState", "navInstruction", "navRoute", }); Params params; From 5c5bb0be1197408ade793b49d731571ba831283a Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 27 May 2022 18:12:18 -0700 Subject: [PATCH 05/29] Hyundai longitudinal: display lead on HUD (#24667) * use leadVisible * add comment --- selfdrive/car/hyundai/carcontroller.py | 5 ++--- selfdrive/car/hyundai/hyundaican.py | 4 ++-- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 170624737d..fb3579464a 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -114,11 +114,10 @@ class CarController: accel = clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) - lead_visible = False stopping = actuators.longControlState == LongCtrlState.stopping set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH) - can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2), lead_visible, - set_speed_in_units, stopping, CS.out.gasPressed)) + can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2), + hud_control.leadVisible, set_speed_in_units, stopping, CS.out.gasPressed)) self.accel = accel # 20 Hz LFA MFA message diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index d03eb135fc..53499053e0 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -86,8 +86,8 @@ def create_acc_commands(packer, enabled, accel, jerk, idx, lead_visible, set_spe "TauGapSet": 4, "VSetDis": set_speed if enabled else 0, "AliveCounterACC": idx % 0x10, - "ObjValid": 1 if lead_visible else 0, - "ACC_ObjStatus": 1 if lead_visible else 0, + "ObjValid": 0, # TODO: these two bits may allow for better longitudinal control + "ACC_ObjStatus": 0, "ACC_ObjLatPos": 0, "ACC_ObjRelSpd": 0, "ACC_ObjDist": 0, From 8eb9e8fc1a94013a60b0e69f7817b1c491cb1a33 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 29 May 2022 20:59:58 -0700 Subject: [PATCH 06/29] Toyota: add missing engine FW version for 2022 RAV4 Hybrid (#24677) add missing fw version --- selfdrive/car/toyota/values.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index b170592744..22089f4b6c 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -1374,6 +1374,7 @@ FW_VERSIONS = { b'\x01896634A08000\x00\x00\x00\x00', b'\x01896634A61000\x00\x00\x00\x00', b'\x01896634A02001\x00\x00\x00\x00', + b'\x01896634A03000\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F0R01100\x00\x00\x00\x00', From 44bffc1b3b2c8a3f661d8ab28ee8765a1bc2219c Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Mon, 30 May 2022 11:36:11 +0200 Subject: [PATCH 07/29] Remove encoderd service from PC. Fixes Sim (#24676) * Disable encoderd completely in sim --- selfdrive/manager/process_config.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index ad3cabbbd9..fd4f9a647c 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -24,7 +24,7 @@ procs = [ NativeProcess("clocksd", "selfdrive/clocksd", ["./clocksd"]), NativeProcess("dmonitoringmodeld", "selfdrive/modeld", ["./dmonitoringmodeld"], enabled=(not PC or WEBCAM), callback=driverview), NativeProcess("logcatd", "selfdrive/logcatd", ["./logcatd"]), - NativeProcess("encoderd", "selfdrive/loggerd", ["./encoderd"]), + NativeProcess("encoderd", "selfdrive/loggerd", ["./encoderd"], enabled=(not PC or WEBCAM)), NativeProcess("loggerd", "selfdrive/loggerd", ["./loggerd"], onroad=False, callback=logging), NativeProcess("modeld", "selfdrive/modeld", ["./modeld"]), NativeProcess("navd", "selfdrive/ui/navd", ["./navd"], offroad=True), From 85b07bf3e878d3e430b5a857b86065588fc2d1fb Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 30 May 2022 11:49:15 +0200 Subject: [PATCH 08/29] sim no encoderd, move to BLOCK env variable --- selfdrive/manager/process_config.py | 2 +- tools/sim/launch_openpilot.sh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index fd4f9a647c..ad3cabbbd9 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -24,7 +24,7 @@ procs = [ NativeProcess("clocksd", "selfdrive/clocksd", ["./clocksd"]), NativeProcess("dmonitoringmodeld", "selfdrive/modeld", ["./dmonitoringmodeld"], enabled=(not PC or WEBCAM), callback=driverview), NativeProcess("logcatd", "selfdrive/logcatd", ["./logcatd"]), - NativeProcess("encoderd", "selfdrive/loggerd", ["./encoderd"], enabled=(not PC or WEBCAM)), + NativeProcess("encoderd", "selfdrive/loggerd", ["./encoderd"]), NativeProcess("loggerd", "selfdrive/loggerd", ["./loggerd"], onroad=False, callback=logging), NativeProcess("modeld", "selfdrive/modeld", ["./modeld"]), NativeProcess("navd", "selfdrive/ui/navd", ["./navd"], offroad=True), diff --git a/tools/sim/launch_openpilot.sh b/tools/sim/launch_openpilot.sh index f5bac8a864..82fc4a71ac 100755 --- a/tools/sim/launch_openpilot.sh +++ b/tools/sim/launch_openpilot.sh @@ -5,7 +5,7 @@ export NOBOARD="1" export SIMULATION="1" export FINGERPRINT="HONDA CIVIC 2016" -export BLOCK="camerad,loggerd" +export BLOCK="camerad,loggerd,encoderd" DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" cd ../../selfdrive/manager && exec ./manager.py From e72d6b5689dd95f5fd7a6558c72c2e02b37f8b12 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 30 May 2022 15:15:51 +0200 Subject: [PATCH 09/29] navd: rewrite in python (#24621) * navd: start python rewrite * use enum * send empty instruction packet * parse banner * cleanup * switch to coordinate class * add segment transition logic * add recompute logic * cleanup old navd code * split out helpers * cleanup release files * fix typo * fix docs * add typing to helpers * small fixes * move outside of ui * get last pos from param * add ui restart detection * check running * send route * set navInstruction to invalid with no active route * whitespace * do not overwrite response and use ratekeeper * reuse params object * remove navd exception --- common/api/__init__.py | 4 +- docs/c_docs.rst | 4 - release/files_common | 5 + release/files_pc | 2 - release/files_tici | 8 - selfdrive/manager/process_config.py | 2 +- selfdrive/manager/test/test_manager.py | 2 +- selfdrive/navd/__init__.py | 0 selfdrive/navd/helpers.py | 162 ++++++++ selfdrive/navd/navd.py | 250 ++++++++++++ selfdrive/ui/SConscript | 8 - selfdrive/ui/navd/.gitignore | 1 - selfdrive/ui/navd/main.cc | 45 --- selfdrive/ui/navd/map_renderer.cc | 200 ---------- selfdrive/ui/navd/map_renderer.h | 49 --- selfdrive/ui/navd/map_renderer.py | 78 ---- selfdrive/ui/navd/navd | 4 - selfdrive/ui/navd/route_engine.cc | 359 ------------------ selfdrive/ui/navd/route_engine.h | 62 --- selfdrive/ui/qt/maps/map_helpers.cc | 95 ----- selfdrive/ui/qt/maps/map_helpers.h | 3 - selfdrive/ui/ui | 1 - selfdrive/ui/watch3.cc | 2 - third_party/qt-plugins/build_qtlocation.sh | 8 - .../x86_64/geoservices/.gitattributes | 1 - .../geoservices/libqtgeoservices_mapbox.so | 3 - 26 files changed, 421 insertions(+), 937 deletions(-) create mode 100644 selfdrive/navd/__init__.py create mode 100644 selfdrive/navd/helpers.py create mode 100755 selfdrive/navd/navd.py delete mode 100644 selfdrive/ui/navd/.gitignore delete mode 100644 selfdrive/ui/navd/main.cc delete mode 100644 selfdrive/ui/navd/map_renderer.cc delete mode 100644 selfdrive/ui/navd/map_renderer.h delete mode 100755 selfdrive/ui/navd/map_renderer.py delete mode 100755 selfdrive/ui/navd/navd delete mode 100644 selfdrive/ui/navd/route_engine.cc delete mode 100644 selfdrive/ui/navd/route_engine.h delete mode 100755 third_party/qt-plugins/build_qtlocation.sh delete mode 100644 third_party/qt-plugins/x86_64/geoservices/.gitattributes delete mode 100755 third_party/qt-plugins/x86_64/geoservices/libqtgeoservices_mapbox.so diff --git a/common/api/__init__.py b/common/api/__init__.py index 8b83dfc641..fd2e70910e 100644 --- a/common/api/__init__.py +++ b/common/api/__init__.py @@ -22,13 +22,13 @@ class Api(): def request(self, method, endpoint, timeout=None, access_token=None, **params): return api_get(endpoint, method=method, timeout=timeout, access_token=access_token, **params) - def get_token(self): + def get_token(self, expiry_hours=1): now = datetime.utcnow() payload = { 'identity': self.dongle_id, 'nbf': now, 'iat': now, - 'exp': now + timedelta(hours=1) + 'exp': now + timedelta(hours=expiry_hours) } token = jwt.encode(payload, self.private_key, algorithm='RS256') if isinstance(token, bytes): diff --git a/docs/c_docs.rst b/docs/c_docs.rst index db7100ab27..1e080a826d 100644 --- a/docs/c_docs.rst +++ b/docs/c_docs.rst @@ -50,10 +50,6 @@ soundd .. autodoxygenindex:: :project: selfdrive_ui_soundd -navd -"""" -.. autodoxygenindex:: - :project: selfdrive_ui_navd replay """""" diff --git a/release/files_common b/release/files_common index e254abec66..982194d774 100644 --- a/release/files_common +++ b/release/files_common @@ -293,6 +293,9 @@ selfdrive/ui/qt/widgets/*.h selfdrive/ui/replay/*.cc selfdrive/ui/replay/*.h +selfdrive/ui/qt/maps/*.cc +selfdrive/ui/qt/maps/*.h + selfdrive/camerad/SConscript selfdrive/camerad/main.cc @@ -362,6 +365,8 @@ selfdrive/modeld/runners/run.h selfdrive/monitoring/dmonitoringd.py selfdrive/monitoring/driver_monitor.py +selfdrive/navd/*.py + selfdrive/assets/.gitignore selfdrive/assets/assets.qrc selfdrive/assets/*.png diff --git a/release/files_pc b/release/files_pc index e401badb80..01ecae4327 100644 --- a/release/files_pc +++ b/release/files_pc @@ -2,8 +2,6 @@ selfdrive/modeld/runners/onnx* third_party/mapbox-gl-native-qt/x86_64/*.so -third_party/qt-plugins/x86_64/geoservices/*.so - third_party/libyuv/x64/** third_party/snpe/x86_64/** third_party/snpe/x86_64-linux-clang/** diff --git a/release/files_tici b/release/files_tici index 75abc13abc..ee1ac63c34 100644 --- a/release/files_tici +++ b/release/files_tici @@ -13,11 +13,3 @@ selfdrive/camerad/cameras/real_debayer.cl selfdrive/ui/qt/spinner_larch64 selfdrive/ui/qt/text_larch64 -selfdrive/ui/qt/maps/*.cc -selfdrive/ui/qt/maps/*.h - -selfdrive/ui/navd/*.cc -selfdrive/ui/navd/*.h -selfdrive/ui/navd/navd -selfdrive/ui/navd/.gitignore - diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index ad3cabbbd9..7f31d837b5 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -27,7 +27,6 @@ procs = [ NativeProcess("encoderd", "selfdrive/loggerd", ["./encoderd"]), NativeProcess("loggerd", "selfdrive/loggerd", ["./loggerd"], onroad=False, callback=logging), NativeProcess("modeld", "selfdrive/modeld", ["./modeld"]), - NativeProcess("navd", "selfdrive/ui/navd", ["./navd"], offroad=True), NativeProcess("proclogd", "selfdrive/proclogd", ["./proclogd"]), NativeProcess("sensord", "selfdrive/sensord", ["./sensord"], enabled=not PC), NativeProcess("ubloxd", "selfdrive/locationd", ["./ubloxd"], enabled=(not PC or WEBCAM)), @@ -40,6 +39,7 @@ procs = [ PythonProcess("deleter", "selfdrive.loggerd.deleter", offroad=True), PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", enabled=(not PC or WEBCAM), callback=driverview), PythonProcess("logmessaged", "selfdrive.logmessaged", offroad=True), + PythonProcess("navd", "selfdrive.navd.navd"), PythonProcess("pandad", "selfdrive.boardd.pandad", offroad=True), PythonProcess("paramsd", "selfdrive.locationd.paramsd"), PythonProcess("plannerd", "selfdrive.controls.plannerd"), diff --git a/selfdrive/manager/test/test_manager.py b/selfdrive/manager/test/test_manager.py index 1750c81b2e..c1a6d4816b 100755 --- a/selfdrive/manager/test/test_manager.py +++ b/selfdrive/manager/test/test_manager.py @@ -50,7 +50,7 @@ class TestManager(unittest.TestCase): self.assertTrue(state.running, f"{p} not running") exit_code = managed_processes[p].stop(retry=False) - if (TICI and p in ['ui', 'navd']): + if (TICI and p in ['ui',]): # TODO: make Qt UI exit gracefully continue diff --git a/selfdrive/navd/__init__.py b/selfdrive/navd/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/navd/helpers.py b/selfdrive/navd/helpers.py new file mode 100644 index 0000000000..89fd5e7e4e --- /dev/null +++ b/selfdrive/navd/helpers.py @@ -0,0 +1,162 @@ +from __future__ import annotations + +import json +import math +from typing import Any, Dict, List, Optional, Tuple + +from common.numpy_fast import clip +from common.params import Params + +EARTH_MEAN_RADIUS = 6371007.2 + + +class Coordinate: + def __init__(self, latitude: float, longitude: float) -> None: + self.latitude = latitude + self.longitude = longitude + + @classmethod + def from_mapbox_tuple(cls, t: Tuple[float, float]) -> Coordinate: + return cls(t[1], t[0]) + + def as_dict(self) -> Dict[str, float]: + return {'latitude': self.latitude, 'longitude': self.longitude} + + def __str__(self) -> str: + return f"({self.latitude}, {self.longitude})" + + def __eq__(self, other) -> bool: + if not isinstance(other, Coordinate): + return False + return (self.latitude == other.latitude) and (self.longitude == other.longitude) + + def __sub__(self, other: Coordinate) -> Coordinate: + return Coordinate(self.latitude - other.latitude, self.longitude - other.longitude) + + def __add__(self, other: Coordinate) -> Coordinate: + return Coordinate(self.latitude + other.latitude, self.longitude + other.longitude) + + def __mul__(self, c: float) -> Coordinate: + return Coordinate(self.latitude * c, self.longitude * c) + + def dot(self, other: Coordinate) -> float: + return self.latitude * other.latitude + self.longitude * other.longitude + + def distance_to(self, other: Coordinate) -> float: + # Haversine formula + dlat = math.radians(other.latitude - self.latitude) + dlon = math.radians(other.longitude - self.longitude) + + haversine_dlat = math.sin(dlat / 2.0) + haversine_dlat *= haversine_dlat + haversine_dlon = math.sin(dlon / 2.0) + haversine_dlon *= haversine_dlon + + y = haversine_dlat \ + + math.cos(math.radians(self.latitude)) \ + * math.cos(math.radians(other.latitude)) \ + * haversine_dlon + x = 2 * math.asin(math.sqrt(y)) + return x * EARTH_MEAN_RADIUS + + +def minimum_distance(a: Coordinate, b: Coordinate, p: Coordinate): + if a.distance_to(b) < 0.01: + return a.distance_to(p) + + ap = p - a + ab = b - a + t = clip(ap.dot(ab) / ab.dot(ab), 0.0, 1.0) + projection = a + ab * t + return projection.distance_to(p) + + +def distance_along_geometry(geometry: List[Coordinate], pos: Coordinate) -> float: + if len(geometry) <= 2: + return geometry[0].distance_to(pos) + + # 1. Find segment that is closest to current position + # 2. Total distance is sum of distance to start of closest segment + # + all previous segments + total_distance = 0.0 + total_distance_closest = 0.0 + closest_distance = 1e9 + + for i in range(len(geometry) - 1): + d = minimum_distance(geometry[i], geometry[i + 1], pos) + + if d < closest_distance: + closest_distance = d + total_distance_closest = total_distance + geometry[i].distance_to(pos) + + total_distance += geometry[i].distance_to(geometry[i + 1]) + + return total_distance_closest + + +def coordinate_from_param(param: str, params: Optional[Params] = None) -> Optional[Coordinate]: + if params is None: + params = Params() + + json_str = params.get(param) + if json_str is None: + return None + + pos = json.loads(json_str) + if 'latitude' not in pos or 'longitude' not in pos: + return None + + return Coordinate(pos['latitude'], pos['longitude']) + + +def string_to_direction(direction: str) -> str: + for d in ['left', 'right', 'straight']: + if d in direction: + return d + return 'none' + + +def parse_banner_instructions(instruction: Any, banners: Any, distance_to_maneuver: float = 0.0) -> None: + if not len(banners): + return + + current_banner = banners[0] + + # A segment can contain multiple banners, find one that we need to show now + for banner in banners: + if distance_to_maneuver < banner['distanceAlongGeometry']: + current_banner = banner + + # Only show banner when close enough to maneuver + instruction.showFull = distance_to_maneuver < current_banner['distanceAlongGeometry'] + + # Primary + p = current_banner['primary'] + if 'text' in p: + instruction.maneuverPrimaryText = p['text'] + if 'type' in p: + instruction.maneuverType = p['type'] + if 'modifier' in p: + instruction.maneuverModifier = p['modifier'] + + # Secondary + if 'secondary' in current_banner: + instruction.maneuverSecondaryText = current_banner['secondary']['text'] + + # Lane lines + if 'sub' in current_banner: + lanes = [] + for component in current_banner['sub']['components']: + if component['type'] != 'lane': + continue + + lane = { + 'active': component['active'], + 'directions': [string_to_direction(d) for d in component['directions']], + } + + if 'active_direction' in component: + lane['activeDirection'] = string_to_direction(component['active_direction']) + + lanes.append(lane) + instruction.lanes = lanes diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py new file mode 100755 index 0000000000..e063773d52 --- /dev/null +++ b/selfdrive/navd/navd.py @@ -0,0 +1,250 @@ +#!/usr/bin/env python3 +import math +import os +import threading + +import requests + +import cereal.messaging as messaging +from cereal import log +from common.api import Api +from common.params import Params +from common.realtime import Ratekeeper +from selfdrive.swaglog import cloudlog +from selfdrive.navd.helpers import (Coordinate, coordinate_from_param, + distance_along_geometry, + minimum_distance, + parse_banner_instructions) + +REROUTE_DISTANCE = 25 +MANEUVER_TRANSITION_THRESHOLD = 10 + + +class RouteEngine: + def __init__(self, sm, pm): + self.sm = sm + self.pm = pm + + self.params = Params() + + # Get last gps position from params + self.last_position = coordinate_from_param("LastGPSPosition", self.params) + self.last_bearing = None + + self.gps_ok = False + + self.nav_destination = None + self.step_idx = None + self.route = None + self.route_geometry = None + + self.recompute_backoff = 0 + self.recompute_countdown = 0 + + self.ui_pid = None + + if "MAPBOX_TOKEN" in os.environ: + self.mapbox_token = os.environ["MAPBOX_TOKEN"] + self.mapbox_host = "https://api.mapbox.com" + else: + self.mapbox_token = Api(self.params.get("DongleId", encoding='utf8')).get_token(expiry_hours=4 * 7 * 24) + self.mapbox_host = "https://maps.comma.ai" + + def update(self): + self.sm.update(0) + + if self.sm.updated["managerState"]: + ui_pid = [p.pid for p in self.sm["managerState"].processes if p.name == "ui" and p.running] + if ui_pid: + if self.ui_pid and self.ui_pid != ui_pid[0]: + cloudlog.warning("UI restarting, sending route") + threading.Timer(5.0, self.send_route).start() + self.ui_pid = ui_pid[0] + + self.update_location() + self.recompute_route() + self.send_instruction() + + def update_location(self): + location = self.sm['liveLocationKalman'] + self.gps_ok = location.gpsOK + + localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid + + if localizer_valid: + self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2]) + self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1]) + + def recompute_route(self): + if self.last_position is None: + return + + new_destination = coordinate_from_param("NavDestination", self.params) + if new_destination is None: + self.clear_route() + return + + should_recompute = self.should_recompute() + if new_destination != self.nav_destination: + cloudlog.warning(f"Got new destination from NavDestination param {new_destination}") + should_recompute = True + + # Don't recompute when GPS drifts in tunnels + if not self.gps_ok and self.step_idx is not None: + return + + if self.recompute_countdown == 0 and should_recompute: + self.recompute_countdown = 2**self.recompute_backoff + self.recompute_backoff = min(6, self.recompute_backoff + 1) + self.calculate_route(new_destination) + else: + self.recompute_countdown = max(0, self.recompute_countdown - 1) + + def calculate_route(self, destination): + cloudlog.warning(f"Calculating route {self.last_position} -> {destination}") + self.nav_destination = destination + + params = { + 'access_token': self.mapbox_token, + # 'annotations': 'maxspeed', + 'geometries': 'geojson', + 'overview': 'full', + 'steps': 'true', + 'banner_instructions': 'true', + 'alternatives': 'false', + } + + if self.last_bearing is not None: + params['bearings'] = f"{(self.last_bearing + 360) % 360:.0f},90;" + + url = self.mapbox_host + f'/directions/v5/mapbox/driving-traffic/{self.last_position.longitude},{self.last_position.latitude};{destination.longitude},{destination.latitude}' + resp = requests.get(url, params=params) + + if resp.status_code == 200: + r = resp.json() + if len(r['routes']): + self.route = r['routes'][0]['legs'][0]['steps'] + self.route_geometry = [] + + # Convert coordinates + for step in self.route: + self.route_geometry.append([Coordinate.from_mapbox_tuple(c) for c in step['geometry']['coordinates']]) + + self.step_idx = 0 + else: + cloudlog.warning("Got empty route response") + self.clear_route() + + else: + cloudlog.warning(f"Got error in route reply {resp.status_code}") + self.clear_route() + + self.send_route() + + def send_instruction(self): + msg = messaging.new_message('navInstruction') + + if self.step_idx is None: + msg.valid = False + self.pm.send('navInstruction', msg) + return + + step = self.route[self.step_idx] + geometry = self.route_geometry[self.step_idx] + along_geometry = distance_along_geometry(geometry, self.last_position) + distance_to_maneuver_along_geometry = step['distance'] - along_geometry + + # Current instruction + msg.navInstruction.maneuverDistance = distance_to_maneuver_along_geometry + parse_banner_instructions(msg.navInstruction, step['bannerInstructions'], distance_to_maneuver_along_geometry) + + # Compute total remaining time and distance + remaning = 1.0 - along_geometry / max(step['distance'], 1) + total_distance = step['distance'] * remaning + total_time = step['duration'] * remaning + total_time_typical = step['duration_typical'] * remaning + + # Add up totals for future steps + for i in range(self.step_idx + 1, len(self.route)): + total_distance += self.route[i]['distance'] + total_time += self.route[i]['duration'] + total_time_typical += self.route[i]['duration_typical'] + + msg.navInstruction.distanceRemaining = total_distance + msg.navInstruction.timeRemaining = total_time + msg.navInstruction.timeRemainingTypical = total_time_typical + + self.pm.send('navInstruction', msg) + + # Transition to next route segment + if distance_to_maneuver_along_geometry < -MANEUVER_TRANSITION_THRESHOLD: + if self.step_idx + 1 < len(self.route): + self.step_idx += 1 + self.recompute_backoff = 0 + self.recompute_countdown = 0 + else: + cloudlog.warning("Destination reached") + Params().delete("NavDestination") + + # Clear route if driving away from destination + dist = self.nav_destination.distance_to(self.last_position) + if dist > REROUTE_DISTANCE: + self.clear_route() + + def send_route(self): + coords = [] + + if self.route is not None: + for path in self.route_geometry: + coords += [c.as_dict() for c in path] + + msg = messaging.new_message('navRoute') + msg.navRoute.coordinates = coords + self.pm.send('navRoute', msg) + + def clear_route(self): + self.route = None + self.route_geometry = None + self.step_idx = None + self.nav_destination = None + + def should_recompute(self): + if self.step_idx is None or self.route is None: + return True + + # Don't recompute in last segment, assume destination is reached + if self.step_idx == len(self.route) - 1: + return False + + # Compute closest distance to all line segments in the current path + min_d = REROUTE_DISTANCE + 1 + path = self.route_geometry[self.step_idx] + for i in range(len(path) - 1): + a = path[i] + b = path[i + 1] + + if a.distance_to(b) < 1.0: + continue + + min_d = min(min_d, minimum_distance(a, b, self.last_position)) + + return min_d > REROUTE_DISTANCE + + # TODO: Check for going wrong way in segment + + +def main(sm=None, pm=None): + if sm is None: + sm = messaging.SubMaster(['liveLocationKalman', 'managerState']) + if pm is None: + pm = messaging.PubMaster(['navInstruction', 'navRoute']) + + rk = Ratekeeper(1.0) + route_engine = RouteEngine(sm, pm) + while True: + route_engine.update() + rk.keep_time() + + +if __name__ == "__main__": + main() diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 1ea13d5564..6b9db34b4d 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -118,11 +118,3 @@ if arch in ['x86_64', 'Darwin'] or GetOption('extras'): if GetOption('test'): qt_env.Program('replay/tests/test_replay', ['replay/tests/test_runner.cc', 'replay/tests/test_replay.cc'], LIBS=[replay_libs]) - -# navd -if maps: - navd_src = ["navd/main.cc", "navd/route_engine.cc", "navd/map_renderer.cc"] - qt_env.Program("navd/_navd", navd_src, LIBS=qt_libs + ['common', 'json11']) - - if GetOption('extras'): - qt_env.SharedLibrary("navd/map_renderer", ["navd/map_renderer.cc"], LIBS=qt_libs + ['common', 'messaging']) diff --git a/selfdrive/ui/navd/.gitignore b/selfdrive/ui/navd/.gitignore deleted file mode 100644 index 0fa7b173e4..0000000000 --- a/selfdrive/ui/navd/.gitignore +++ /dev/null @@ -1 +0,0 @@ -_navd diff --git a/selfdrive/ui/navd/main.cc b/selfdrive/ui/navd/main.cc deleted file mode 100644 index fe354b7b7d..0000000000 --- a/selfdrive/ui/navd/main.cc +++ /dev/null @@ -1,45 +0,0 @@ -#include -#include -#include -#include -#include - -#include "selfdrive/ui/qt/util.h" -#include "selfdrive/ui/qt/maps/map_helpers.h" -#include "selfdrive/ui/navd/route_engine.h" -#include "selfdrive/ui/navd/map_renderer.h" -#include "selfdrive/hardware/hw.h" -#include "common/params.h" - -void sigHandler(int s) { - qInfo() << "Shutting down"; - std::signal(s, SIG_DFL); - - qApp->quit(); -} - - -int main(int argc, char *argv[]) { - qInstallMessageHandler(swagLogMessageHandler); - - QApplication app(argc, argv); - std::signal(SIGINT, sigHandler); - std::signal(SIGTERM, sigHandler); - - QCommandLineParser parser; - parser.setApplicationDescription("Navigation server. Runs stand-alone, or using pre-computer route"); - parser.addHelpOption(); - parser.process(app); - const QStringList args = parser.positionalArguments(); - - - RouteEngine* route_engine = new RouteEngine(); - - if (Params().getBool("NavdRender")) { - MapRenderer * m = new MapRenderer(get_mapbox_settings()); - QObject::connect(route_engine, &RouteEngine::positionUpdated, m, &MapRenderer::updatePosition); - QObject::connect(route_engine, &RouteEngine::routeUpdated, m, &MapRenderer::updateRoute); - } - - return app.exec(); -} diff --git a/selfdrive/ui/navd/map_renderer.cc b/selfdrive/ui/navd/map_renderer.cc deleted file mode 100644 index 8d2a8810c9..0000000000 --- a/selfdrive/ui/navd/map_renderer.cc +++ /dev/null @@ -1,200 +0,0 @@ -#include "selfdrive/ui/navd/map_renderer.h" - -#include -#include -#include - -#include "selfdrive/ui/qt/maps/map_helpers.h" -#include "common/timing.h" - -const float ZOOM = 13.5; // Don't go below 13 or features will start to disappear -const int WIDTH = 256; -const int HEIGHT = WIDTH; - -const int NUM_VIPC_BUFFERS = 4; - -MapRenderer::MapRenderer(const QMapboxGLSettings &settings, bool enable_vipc) : m_settings(settings) { - QSurfaceFormat fmt; - fmt.setRenderableType(QSurfaceFormat::OpenGLES); - - ctx = std::make_unique(); - ctx->setFormat(fmt); - ctx->create(); - assert(ctx->isValid()); - - surface = std::make_unique(); - surface->setFormat(ctx->format()); - surface->create(); - - ctx->makeCurrent(surface.get()); - assert(QOpenGLContext::currentContext() == ctx.get()); - - gl_functions.reset(ctx->functions()); - gl_functions->initializeOpenGLFunctions(); - - QOpenGLFramebufferObjectFormat fbo_format; - fbo.reset(new QOpenGLFramebufferObject(WIDTH, HEIGHT, fbo_format)); - - m_map.reset(new QMapboxGL(nullptr, m_settings, fbo->size(), 1)); - m_map->setCoordinateZoom(QMapbox::Coordinate(0, 0), ZOOM); - m_map->setStyleUrl("mapbox://styles/commaai/ckvmksrpd4n0a14pfdo5heqzr"); - m_map->createRenderer(); - - m_map->resize(fbo->size()); - m_map->setFramebufferObject(fbo->handle(), fbo->size()); - gl_functions->glViewport(0, 0, WIDTH, HEIGHT); - - if (enable_vipc) { - qWarning() << "Enabling navd map rendering"; - vipc_server.reset(new VisionIpcServer("navd")); - vipc_server->create_buffers(VisionStreamType::VISION_STREAM_RGB_MAP, NUM_VIPC_BUFFERS, true, WIDTH, HEIGHT); - vipc_server->start_listener(); - - pm.reset(new PubMaster({"navThumbnail"})); - } -} - -void MapRenderer::updatePosition(QMapbox::Coordinate position, float bearing) { - if (m_map.isNull()) { - return; - } - - m_map->setCoordinate(position); - m_map->setBearing(bearing); - update(); -} - -bool MapRenderer::loaded() { - return m_map->isFullyLoaded(); -} - -void MapRenderer::update() { - gl_functions->glClear(GL_COLOR_BUFFER_BIT); - m_map->render(); - gl_functions->glFlush(); - - sendVipc(); -} - -void MapRenderer::sendVipc() { - if (!vipc_server || !loaded()) { - return; - } - - QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor); - uint64_t ts = nanos_since_boot(); - VisionBuf* buf = vipc_server->get_buffer(VisionStreamType::VISION_STREAM_RGB_MAP); - VisionIpcBufExtra extra = { - .frame_id = frame_id, - .timestamp_sof = ts, - .timestamp_eof = ts, - }; - - assert(cap.sizeInBytes() == buf->len); - memcpy(buf->addr, cap.bits(), buf->len); - vipc_server->send(buf, &extra); - - if (frame_id % 100 == 0) { - // Write jpeg into buffer - QByteArray buffer_bytes; - QBuffer buffer(&buffer_bytes); - buffer.open(QIODevice::WriteOnly); - cap.save(&buffer, "JPG", 50); - - kj::Array buffer_kj = kj::heapArray((const capnp::byte*)buffer_bytes.constData(), buffer_bytes.size()); - - // Send thumbnail - MessageBuilder msg; - auto thumbnaild = msg.initEvent().initNavThumbnail(); - thumbnaild.setFrameId(frame_id); - thumbnaild.setTimestampEof(ts); - thumbnaild.setThumbnail(buffer_kj); - pm->send("navThumbnail", msg); - } - - frame_id++; -} - -uint8_t* MapRenderer::getImage() { - QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor); - uint8_t* buf = new uint8_t[cap.sizeInBytes()]; - memcpy(buf, cap.bits(), cap.sizeInBytes()); - - return buf; -} - -void MapRenderer::updateRoute(QList coordinates) { - if (m_map.isNull()) return; - initLayers(); - - auto route_points = coordinate_list_to_collection(coordinates); - QMapbox::Feature feature(QMapbox::Feature::LineStringType, route_points, {}, {}); - QVariantMap navSource; - navSource["type"] = "geojson"; - navSource["data"] = QVariant::fromValue(feature); - m_map->updateSource("navSource", navSource); - m_map->setLayoutProperty("navLayer", "visibility", "visible"); -} - -void MapRenderer::initLayers() { - if (!m_map->layerExists("navLayer")) { - QVariantMap nav; - nav["id"] = "navLayer"; - nav["type"] = "line"; - nav["source"] = "navSource"; - m_map->addLayer(nav, "road-intersection"); - m_map->setPaintProperty("navLayer", "line-color", QColor("blue")); - m_map->setPaintProperty("navLayer", "line-width", 3); - m_map->setLayoutProperty("navLayer", "line-cap", "round"); - } -} - -MapRenderer::~MapRenderer() { -} - -extern "C" { - MapRenderer* map_renderer_init() { - char *argv[] = { - (char*)"navd", - nullptr - }; - int argc = 0; - QApplication *app = new QApplication(argc, argv); - assert(app); - - QMapboxGLSettings settings; - settings.setApiBaseUrl(MAPS_HOST); - settings.setAccessToken(get_mapbox_token()); - - return new MapRenderer(settings, false); - } - - void map_renderer_update_position(MapRenderer *inst, float lat, float lon, float bearing) { - inst->updatePosition({lat, lon}, bearing); - QApplication::processEvents(); - } - - void map_renderer_update_route(MapRenderer *inst, char* polyline) { - inst->updateRoute(polyline_to_coordinate_list(QString::fromUtf8(polyline))); - } - - void map_renderer_update(MapRenderer *inst) { - inst->update(); - } - - void map_renderer_process(MapRenderer *inst) { - QApplication::processEvents(); - } - - bool map_renderer_loaded(MapRenderer *inst) { - return inst->loaded(); - } - - uint8_t * map_renderer_get_image(MapRenderer *inst) { - return inst->getImage(); - } - - void map_renderer_free_image(MapRenderer *inst, uint8_t * buf) { - delete[] buf; - } -} diff --git a/selfdrive/ui/navd/map_renderer.h b/selfdrive/ui/navd/map_renderer.h deleted file mode 100644 index 1746e76695..0000000000 --- a/selfdrive/ui/navd/map_renderer.h +++ /dev/null @@ -1,49 +0,0 @@ -#pragma once - -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "cereal/visionipc/visionipc_server.h" -#include "cereal/messaging/messaging.h" - - -class MapRenderer : public QObject { - Q_OBJECT - -public: - MapRenderer(const QMapboxGLSettings &, bool enable_vipc=true); - uint8_t* getImage(); - void update(); - bool loaded(); - ~MapRenderer(); - - -private: - std::unique_ptr ctx; - std::unique_ptr surface; - std::unique_ptr gl_functions; - std::unique_ptr fbo; - - std::unique_ptr vipc_server; - std::unique_ptr pm; - void sendVipc(); - - QMapboxGLSettings m_settings; - QScopedPointer m_map; - - void initLayers(); - - uint32_t frame_id = 0; - -public slots: - void updatePosition(QMapbox::Coordinate position, float bearing); - void updateRoute(QList coordinates); -}; diff --git a/selfdrive/ui/navd/map_renderer.py b/selfdrive/ui/navd/map_renderer.py deleted file mode 100755 index 28d006841b..0000000000 --- a/selfdrive/ui/navd/map_renderer.py +++ /dev/null @@ -1,78 +0,0 @@ -#!/usr/bin/env python3 -# You might need to uninstall the PyQt5 pip package to avoid conflicts - -import os -import time -from cffi import FFI - -from common.ffi_wrapper import suffix -from common.basedir import BASEDIR - -HEIGHT = WIDTH = 256 - - -def get_ffi(): - lib = os.path.join(BASEDIR, "selfdrive", "ui", "navd", "libmap_renderer" + suffix()) - - ffi = FFI() - ffi.cdef(""" -void* map_renderer_init(); -void map_renderer_update_position(void *inst, float lat, float lon, float bearing); -void map_renderer_update_route(void *inst, char *polyline); -void map_renderer_update(void *inst); -void map_renderer_process(void *inst); -bool map_renderer_loaded(void *inst); -uint8_t* map_renderer_get_image(void *inst); -void map_renderer_free_image(void *inst, uint8_t *buf); -""") - return ffi, ffi.dlopen(lib) - - -def wait_ready(lib, renderer): - while not lib.map_renderer_loaded(renderer): - lib.map_renderer_update(renderer) - - # The main qt app is not execed, so we need to periodically process events for e.g. network requests - lib.map_renderer_process(renderer) - - time.sleep(0.01) - - -def get_image(lib, renderer): - buf = lib.map_renderer_get_image(renderer) - r = list(buf[0:3 * WIDTH * HEIGHT]) - lib.map_renderer_free_image(renderer, buf) - - # Convert to numpy - r = np.asarray(r) - return r.reshape((WIDTH, HEIGHT, 3)) - - -if __name__ == "__main__": - import matplotlib.pyplot as plt - import numpy as np - - ffi, lib = get_ffi() - renderer = lib.map_renderer_init() - wait_ready(lib, renderer) - - geometry = r"{yxk}@|obn~Eg@@eCFqc@J{RFw@?kA@gA?q|@Riu@NuJBgi@ZqVNcRBaPBkG@iSD{I@_H@cH?gG@mG@gG?aD@{LDgDDkVVyQLiGDgX@q_@@qI@qKhS{R~[}NtYaDbGoIvLwNfP_b@|f@oFnF_JxHel@bf@{JlIuxAlpAkNnLmZrWqFhFoh@jd@kX|TkJxH_RnPy^|[uKtHoZ~Um`DlkCorC``CuShQogCtwB_ThQcr@fk@sVrWgRhVmSb\\oj@jxA{Qvg@u]tbAyHzSos@xjBeKbWszAbgEc~@~jCuTrl@cYfo@mRn\\_m@v}@ij@jp@om@lk@y|A`pAiXbVmWzUod@xj@wNlTw}@|uAwSn\\kRfYqOdS_IdJuK`KmKvJoOhLuLbHaMzGwO~GoOzFiSrEsOhD}PhCqw@vJmnAxSczA`Vyb@bHk[fFgl@pJeoDdl@}}@zIyr@hG}X`BmUdBcM^aRR}Oe@iZc@mR_@{FScHxAn_@vz@zCzH~GjPxAhDlB~DhEdJlIbMhFfG|F~GlHrGjNjItLnGvQ~EhLnBfOn@p`@AzAAvn@CfC?fc@`@lUrArStCfSxEtSzGxM|ElFlBrOzJlEbDnC~BfDtCnHjHlLvMdTnZzHpObOf^pKla@~G|a@dErg@rCbj@zArYlj@ttJ~AfZh@r]LzYg@`TkDbj@gIdv@oE|i@kKzhA{CdNsEfOiGlPsEvMiDpLgBpHyB`MkB|MmArPg@|N?|P^rUvFz~AWpOCdAkB|PuB`KeFfHkCfGy@tAqC~AsBPkDs@uAiAcJwMe@s@eKkPMoXQux@EuuCoH?eI?Kas@}Dy@wAUkMOgDL" - lib.map_renderer_update_route(renderer, geometry.encode()) - - POSITIONS = [ - (32.71569271952601, -117.16384270868463, 0), (32.71569271952601, -117.16384270868463, 45), # San Diego - (52.378641991483136, 4.902623379456488, 0), (52.378641991483136, 4.902623379456488, 45), # Amsterdam - ] - plt.figure() - - for i, pos in enumerate(POSITIONS): - t = time.time() - lib.map_renderer_update_position(renderer, *pos) - wait_ready(lib, renderer) - - print(f"{pos} took {time.time() - t:.2f} s") - - plt.subplot(2, 2, i + 1) - plt.imshow(get_image(lib, renderer)) - - plt.show() diff --git a/selfdrive/ui/navd/navd b/selfdrive/ui/navd/navd deleted file mode 100755 index ff3103bd97..0000000000 --- a/selfdrive/ui/navd/navd +++ /dev/null @@ -1,4 +0,0 @@ -#!/bin/sh -cd "$(dirname "$0")" -export QT_PLUGIN_PATH="../../../third_party/qt-plugins/$(uname -m)" -exec ./_navd diff --git a/selfdrive/ui/navd/route_engine.cc b/selfdrive/ui/navd/route_engine.cc deleted file mode 100644 index 577f267c9b..0000000000 --- a/selfdrive/ui/navd/route_engine.cc +++ /dev/null @@ -1,359 +0,0 @@ -#include "selfdrive/ui/navd/route_engine.h" - -#include - -#include "selfdrive/ui/qt/maps/map.h" -#include "selfdrive/ui/qt/maps/map_helpers.h" -#include "selfdrive/ui/qt/api.h" - -#include "common/params.h" - -const qreal REROUTE_DISTANCE = 25; -const float MANEUVER_TRANSITION_THRESHOLD = 10; - -static float get_time_typical(const QGeoRouteSegment &segment) { - auto maneuver = segment.maneuver(); - auto attrs = maneuver.extendedAttributes(); - return attrs.contains("mapbox.duration_typical") ? attrs["mapbox.duration_typical"].toDouble() : segment.travelTime(); -} - -static cereal::NavInstruction::Direction string_to_direction(QString d) { - if (d.contains("left")) { - return cereal::NavInstruction::Direction::LEFT; - } else if (d.contains("right")) { - return cereal::NavInstruction::Direction::RIGHT; - } else if (d.contains("straight")) { - return cereal::NavInstruction::Direction::STRAIGHT; - } - - return cereal::NavInstruction::Direction::NONE; -} - -static void parse_banner(cereal::NavInstruction::Builder &instruction, const QMap &banner, bool full) { - QString primary_str, secondary_str; - - auto p = banner["primary"].toMap(); - primary_str += p["text"].toString(); - - instruction.setShowFull(full); - - if (p.contains("type")) { - instruction.setManeuverType(p["type"].toString().toStdString()); - } - - if (p.contains("modifier")) { - instruction.setManeuverModifier(p["modifier"].toString().toStdString()); - } - - if (banner.contains("secondary")) { - auto s = banner["secondary"].toMap(); - secondary_str += s["text"].toString(); - } - - instruction.setManeuverPrimaryText(primary_str.toStdString()); - instruction.setManeuverSecondaryText(secondary_str.toStdString()); - - if (banner.contains("sub")) { - auto s = banner["sub"].toMap(); - auto components = s["components"].toList(); - - size_t num_lanes = 0; - for (auto &c : components) { - auto cc = c.toMap(); - if (cc["type"].toString() == "lane") { - num_lanes += 1; - } - } - - auto lanes = instruction.initLanes(num_lanes); - - size_t i = 0; - for (auto &c : components) { - auto cc = c.toMap(); - if (cc["type"].toString() == "lane") { - auto lane = lanes[i]; - lane.setActive(cc["active"].toBool()); - - if (cc.contains("active_direction")) { - lane.setActiveDirection(string_to_direction(cc["active_direction"].toString())); - } - - auto directions = lane.initDirections(cc["directions"].toList().size()); - - size_t j = 0; - for (auto &dir : cc["directions"].toList()) { - directions.set(j, string_to_direction(dir.toString())); - j++; - } - - - i++; - } - } - } -} - -RouteEngine::RouteEngine() { - sm = new SubMaster({"liveLocationKalman", "managerState"}); - pm = new PubMaster({"navInstruction", "navRoute"}); - - // Timers - route_timer = new QTimer(this); - QObject::connect(route_timer, SIGNAL(timeout()), this, SLOT(routeUpdate())); - route_timer->start(1000); - - msg_timer = new QTimer(this); - QObject::connect(msg_timer, SIGNAL(timeout()), this, SLOT(msgUpdate())); - msg_timer->start(50); - - // Build routing engine - QVariantMap parameters; - parameters["mapbox.access_token"] = get_mapbox_token(); - parameters["mapbox.directions_api_url"] = MAPS_HOST + "/directions/v5/mapbox/"; - - geoservice_provider = new QGeoServiceProvider("mapbox", parameters); - routing_manager = geoservice_provider->routingManager(); - if (routing_manager == nullptr) { - qWarning() << geoservice_provider->errorString(); - assert(routing_manager); - } - QObject::connect(routing_manager, &QGeoRoutingManager::finished, this, &RouteEngine::routeCalculated); - - // Get last gps position from params - auto last_gps_position = coordinate_from_param("LastGPSPosition"); - if (last_gps_position) { - last_position = *last_gps_position; - } -} - -void RouteEngine::msgUpdate() { - sm->update(1000); - if (!sm->updated("liveLocationKalman")) { - active = false; - return; - } - - - if (sm->updated("managerState")) { - for (auto const &p : (*sm)["managerState"].getManagerState().getProcesses()) { - if (p.getName() == "ui" && p.getRunning()) { - if (ui_pid && *ui_pid != p.getPid()){ - qWarning() << "UI restarting, sending route"; - QTimer::singleShot(5000, this, &RouteEngine::sendRoute); - } - ui_pid = p.getPid(); - } - } - } - - auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman(); - auto pos = location.getPositionGeodetic(); - auto orientation = location.getCalibratedOrientationNED(); - - gps_ok = location.getGpsOK(); - - localizer_valid = (location.getStatus() == cereal::LiveLocationKalman::Status::VALID) && pos.getValid(); - - if (localizer_valid) { - last_bearing = RAD2DEG(orientation.getValue()[2]); - last_position = QMapbox::Coordinate(pos.getValue()[0], pos.getValue()[1]); - emit positionUpdated(*last_position, *last_bearing); - } - - active = true; -} - -void RouteEngine::routeUpdate() { - if (!active) { - return; - } - - recomputeRoute(); - - MessageBuilder msg; - cereal::Event::Builder evt = msg.initEvent(segment.isValid()); - cereal::NavInstruction::Builder instruction = evt.initNavInstruction(); - - // Show route instructions - if (segment.isValid()) { - auto cur_maneuver = segment.maneuver(); - auto attrs = cur_maneuver.extendedAttributes(); - if (cur_maneuver.isValid() && attrs.contains("mapbox.banner_instructions")) { - float along_geometry = distance_along_geometry(segment.path(), to_QGeoCoordinate(*last_position)); - float distance_to_maneuver_along_geometry = segment.distance() - along_geometry; - - auto banners = attrs["mapbox.banner_instructions"].toList(); - if (banners.size()) { - auto banner = banners[0].toMap(); - - for (auto &b : banners) { - auto bb = b.toMap(); - if (distance_to_maneuver_along_geometry < bb["distance_along_geometry"].toDouble()) { - banner = bb; - } - } - - instruction.setManeuverDistance(distance_to_maneuver_along_geometry); - parse_banner(instruction, banner, distance_to_maneuver_along_geometry < banner["distance_along_geometry"].toDouble()); - - // ETA - float progress = distance_along_geometry(segment.path(), to_QGeoCoordinate(*last_position)) / segment.distance(); - float total_distance = segment.distance() * (1.0 - progress); - float total_time = segment.travelTime() * (1.0 - progress); - float total_time_typical = get_time_typical(segment) * (1.0 - progress); - - auto s = segment.nextRouteSegment(); - while (s.isValid()) { - total_distance += s.distance(); - total_time += s.travelTime(); - total_time_typical += get_time_typical(s); - - s = s.nextRouteSegment(); - } - instruction.setTimeRemaining(total_time); - instruction.setTimeRemainingTypical(total_time_typical); - instruction.setDistanceRemaining(total_distance); - } - - // Transition to next route segment - if (distance_to_maneuver_along_geometry < -MANEUVER_TRANSITION_THRESHOLD) { - auto next_segment = segment.nextRouteSegment(); - if (next_segment.isValid()) { - segment = next_segment; - - recompute_backoff = 0; - recompute_countdown = 0; - } else { - qWarning() << "Destination reached"; - Params().remove("NavDestination"); - - // Clear route if driving away from destination - float d = segment.maneuver().position().distanceTo(to_QGeoCoordinate(*last_position)); - if (d > REROUTE_DISTANCE) { - clearRoute(); - } - } - } - } - } - - pm->send("navInstruction", msg); -} - -void RouteEngine::clearRoute() { - route = QGeoRoute(); - segment = QGeoRouteSegment(); - nav_destination = QMapbox::Coordinate(); -} - -bool RouteEngine::shouldRecompute() { - if (!segment.isValid()) { - return true; - } - - // Don't recompute in last segment, assume destination is reached - if (!segment.nextRouteSegment().isValid()) { - return false; - } - - // Compute closest distance to all line segments in the current path - float min_d = REROUTE_DISTANCE + 1; - auto path = segment.path(); - auto cur = to_QGeoCoordinate(*last_position); - for (size_t i = 0; i < path.size() - 1; i++) { - auto a = path[i]; - auto b = path[i+1]; - if (a.distanceTo(b) < 1.0) { - continue; - } - min_d = std::min(min_d, minimum_distance(a, b, cur)); - } - return min_d > REROUTE_DISTANCE; - - // TODO: Check for going wrong way in segment -} - -void RouteEngine::recomputeRoute() { - if (!last_position) { - return; - } - - auto new_destination = coordinate_from_param("NavDestination"); - if (!new_destination) { - clearRoute(); - return; - } - - bool should_recompute = shouldRecompute(); - if (*new_destination != nav_destination) { - qWarning() << "Got new destination from NavDestination param" << *new_destination; - should_recompute = true; - } - - if (!gps_ok && segment.isValid()) return; // Don't recompute when gps drifts in tunnels - - if (recompute_countdown == 0 && should_recompute) { - recompute_countdown = std::pow(2, recompute_backoff); - recompute_backoff = std::min(6, recompute_backoff + 1); - calculateRoute(*new_destination); - } else { - recompute_countdown = std::max(0, recompute_countdown - 1); - } -} - -void RouteEngine::calculateRoute(QMapbox::Coordinate destination) { - qWarning() << "Calculating route" << *last_position << "->" << destination; - - nav_destination = destination; - QGeoRouteRequest request(to_QGeoCoordinate(*last_position), to_QGeoCoordinate(destination)); - request.setFeatureWeight(QGeoRouteRequest::TrafficFeature, QGeoRouteRequest::AvoidFeatureWeight); - - if (last_bearing) { - QVariantMap params; - int bearing = ((int)(*last_bearing) + 360) % 360; - params["bearing"] = bearing; - request.setWaypointsMetadata({params}); - } - - routing_manager->calculateRoute(request); -} - -void RouteEngine::routeCalculated(QGeoRouteReply *reply) { - if (reply->error() == QGeoRouteReply::NoError) { - if (reply->routes().size() != 0) { - qWarning() << "Got route response"; - - route = reply->routes().at(0); - segment = route.firstRouteSegment(); - - auto path = route.path(); - emit routeUpdated(path); - } else { - qWarning() << "Got empty route response"; - } - } else { - qWarning() << "Got error in route reply" << reply->errorString(); - } - - sendRoute(); - - reply->deleteLater(); -} - -void RouteEngine::sendRoute() { - MessageBuilder msg; - cereal::Event::Builder evt = msg.initEvent(); - cereal::NavRoute::Builder nav_route = evt.initNavRoute(); - - auto path = route.path(); - auto coordinates = nav_route.initCoordinates(path.size()); - - size_t i = 0; - for (auto const &c : route.path()) { - coordinates[i].setLatitude(c.latitude()); - coordinates[i].setLongitude(c.longitude()); - i++; - } - - pm->send("navRoute", msg); -} diff --git a/selfdrive/ui/navd/route_engine.h b/selfdrive/ui/navd/route_engine.h deleted file mode 100644 index 33cbc79107..0000000000 --- a/selfdrive/ui/navd/route_engine.h +++ /dev/null @@ -1,62 +0,0 @@ -#pragma once - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "cereal/messaging/messaging.h" - -class RouteEngine : public QObject { - Q_OBJECT - -public: - RouteEngine(); - - SubMaster *sm; - PubMaster *pm; - - QTimer* msg_timer; - QTimer* route_timer; - - std::optional ui_pid; - - // Route - bool gps_ok = false; - QGeoServiceProvider *geoservice_provider; - QGeoRoutingManager *routing_manager; - QGeoRoute route; - QGeoRouteSegment segment; - QMapbox::Coordinate nav_destination; - - // Position - std::optional last_position; - std::optional last_bearing; - bool localizer_valid = false; - - // Route recompute - bool active = false; - int recompute_backoff = 0; - int recompute_countdown = 0; - void calculateRoute(QMapbox::Coordinate destination); - void clearRoute(); - bool shouldRecompute(); - -private slots: - void routeUpdate(); - void msgUpdate(); - void routeCalculated(QGeoRouteReply *reply); - void recomputeRoute(); - void sendRoute(); - -signals: - void positionUpdated(QMapbox::Coordinate position, float bearing); - void routeUpdated(QList coordinates); -}; diff --git a/selfdrive/ui/qt/maps/map_helpers.cc b/selfdrive/ui/qt/maps/map_helpers.cc index be1098994b..83576eb630 100644 --- a/selfdrive/ui/qt/maps/map_helpers.cc +++ b/selfdrive/ui/qt/maps/map_helpers.cc @@ -101,101 +101,6 @@ QMapbox::CoordinatesCollections coordinate_list_to_collection(QList polyline_to_coordinate_list(const QString &polylineString) { - QList path; - if (polylineString.isEmpty()) - return path; - - QByteArray data = polylineString.toLatin1(); - - bool parsingLatitude = true; - - int shift = 0; - int value = 0; - - QGeoCoordinate coord(0, 0); - - for (int i = 0; i < data.length(); ++i) { - unsigned char c = data.at(i) - 63; - - value |= (c & 0x1f) << shift; - shift += 5; - - // another chunk - if (c & 0x20) - continue; - - int diff = (value & 1) ? ~(value >> 1) : (value >> 1); - - if (parsingLatitude) { - coord.setLatitude(coord.latitude() + (double)diff/1e6); - } else { - coord.setLongitude(coord.longitude() + (double)diff/1e6); - path.append(coord); - } - - parsingLatitude = !parsingLatitude; - - value = 0; - shift = 0; - } - - return path; -} - -static QGeoCoordinate sub(QGeoCoordinate v, QGeoCoordinate w) { - return QGeoCoordinate(v.latitude() - w.latitude(), v.longitude() - w.longitude()); -} - -static QGeoCoordinate add(QGeoCoordinate v, QGeoCoordinate w) { - return QGeoCoordinate(v.latitude() + w.latitude(), v.longitude() + w.longitude()); -} - -static QGeoCoordinate mul(QGeoCoordinate v, float c) { - return QGeoCoordinate(c * v.latitude(), c * v.longitude()); -} - -static float dot(QGeoCoordinate v, QGeoCoordinate w) { - return v.latitude() * w.latitude() + v.longitude() * w.longitude(); -} - -float minimum_distance(QGeoCoordinate a, QGeoCoordinate b, QGeoCoordinate p) { - // If a and b are the same coordinate the computation below doesn't work - if (a.distanceTo(b) < 0.01) { - return a.distanceTo(p); - } - - const QGeoCoordinate ap = sub(p, a); - const QGeoCoordinate ab = sub(b, a); - const float t = std::clamp(dot(ap, ab) / dot(ab, ab), 0.0f, 1.0f); - const QGeoCoordinate projection = add(a, mul(ab, t)); - return projection.distanceTo(p); -} - -float distance_along_geometry(QList geometry, QGeoCoordinate pos) { - if (geometry.size() <= 2) { - return geometry[0].distanceTo(pos); - } - - // 1. Find segment that is closest to current position - // 2. Total distance is sum of distance to start of closest segment - // + all previous segments - double total_distance = 0; - double total_distance_closest = 0; - double closest_distance = std::numeric_limits::max(); - - for (int i = 0; i < geometry.size() - 1; i++) { - double d = minimum_distance(geometry[i], geometry[i+1], pos); - if (d < closest_distance) { - closest_distance = d; - total_distance_closest = total_distance + geometry[i].distanceTo(pos); - } - total_distance += geometry[i].distanceTo(geometry[i+1]); - } - - return total_distance_closest; -} - std::optional coordinate_from_param(std::string param) { QString json_str = QString::fromStdString(Params().get(param)); if (json_str.isEmpty()) return {}; diff --git a/selfdrive/ui/qt/maps/map_helpers.h b/selfdrive/ui/qt/maps/map_helpers.h index cda4cd1cfb..344246bb05 100644 --- a/selfdrive/ui/qt/maps/map_helpers.h +++ b/selfdrive/ui/qt/maps/map_helpers.h @@ -24,8 +24,5 @@ QMapbox::CoordinatesCollections model_to_collection( QMapbox::CoordinatesCollections coordinate_to_collection(QMapbox::Coordinate c); QMapbox::CoordinatesCollections capnp_coordinate_list_to_collection(const capnp::List::Reader &coordinate_list); QMapbox::CoordinatesCollections coordinate_list_to_collection(QList coordinate_list); -QList polyline_to_coordinate_list(const QString &polylineString); -float minimum_distance(QGeoCoordinate a, QGeoCoordinate b, QGeoCoordinate p); std::optional coordinate_from_param(std::string param); -float distance_along_geometry(QList geometry, QGeoCoordinate pos); diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 16ddbab050..c9f81c0539 100755 --- a/selfdrive/ui/ui +++ b/selfdrive/ui/ui @@ -1,6 +1,5 @@ #!/bin/sh cd "$(dirname "$0")" export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH" -export QT_PLUGIN_PATH="../../third_party/qt-plugins/$(uname -m)" export QT_DBL_CLICK_DIST=150 exec ./_ui diff --git a/selfdrive/ui/watch3.cc b/selfdrive/ui/watch3.cc index c1d47d040d..00d23ea976 100644 --- a/selfdrive/ui/watch3.cc +++ b/selfdrive/ui/watch3.cc @@ -19,8 +19,6 @@ int main(int argc, char *argv[]) { { QHBoxLayout *hlayout = new QHBoxLayout(); layout->addLayout(hlayout); - // TODO: make mapd output YUV - // hlayout->addWidget(new CameraViewWidget("navd", VISION_STREAM_MAP, false)); hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_ROAD, false)); } diff --git a/third_party/qt-plugins/build_qtlocation.sh b/third_party/qt-plugins/build_qtlocation.sh deleted file mode 100755 index 09e6182d1f..0000000000 --- a/third_party/qt-plugins/build_qtlocation.sh +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env sh - -# Qtlocation plugin with extra fields parsed from api response -cd /tmp -git clone https://github.com/commaai/qtlocation.git -cd qtlocation -qmake -make -j$(nproc) diff --git a/third_party/qt-plugins/x86_64/geoservices/.gitattributes b/third_party/qt-plugins/x86_64/geoservices/.gitattributes deleted file mode 100644 index 51b1d333fe..0000000000 --- a/third_party/qt-plugins/x86_64/geoservices/.gitattributes +++ /dev/null @@ -1 +0,0 @@ -libqtgeoservices_mapbox.so filter=lfs diff=lfs merge=lfs -text diff --git a/third_party/qt-plugins/x86_64/geoservices/libqtgeoservices_mapbox.so b/third_party/qt-plugins/x86_64/geoservices/libqtgeoservices_mapbox.so deleted file mode 100755 index 375fc0e012..0000000000 --- a/third_party/qt-plugins/x86_64/geoservices/libqtgeoservices_mapbox.so +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:5c8bda321dc524e753f67823cb5353358e756cfc1c5328e22e32920e80dd9e9b -size 12166248 From 30e70baf135b405e0a53a3e420cb46bb6a6b602a Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 30 May 2022 15:20:59 +0200 Subject: [PATCH 10/29] bump cereal --- cereal | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cereal b/cereal index 35979b7e2e..93a2560618 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 35979b7e2e52b2e743b2609d11365508760df63b +Subproject commit 93a2560618ef41bc256f7a0331c8b047863d4d69 From b5aed2bf6781cd4d74d28f6f44d24737ec08f0db Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Mon, 30 May 2022 15:25:07 +0200 Subject: [PATCH 11/29] Simulator: Speed up process by removing second camera (#24679) * simulator: support running wide camera only * proper frame id * use param name that makes more sense * do some cleanup * Update tools/sim/bridge.py Co-authored-by: Willem Melching --- common/params.cc | 2 +- selfdrive/controls/plannerd.py | 2 +- selfdrive/locationd/calibrationd.py | 3 +- selfdrive/modeld/modeld.cc | 2 +- .../test/process_replay/process_replay.py | 2 +- selfdrive/ui/qt/onroad.cc | 2 +- selfdrive/ui/ui.cc | 4 +- tools/sim/bridge.py | 42 ++++++++++++------- 8 files changed, 35 insertions(+), 24 deletions(-) diff --git a/common/params.cc b/common/params.cc index 2d4f62f735..2338969600 100644 --- a/common/params.cc +++ b/common/params.cc @@ -104,7 +104,6 @@ std::unordered_map keys = { {"DoReboot", CLEAR_ON_MANAGER_START}, {"DoShutdown", CLEAR_ON_MANAGER_START}, {"DoUninstall", CLEAR_ON_MANAGER_START}, - {"EnableWideCamera", CLEAR_ON_MANAGER_START}, {"EndToEndToggle", PERSISTENT}, {"ForcePowerDown", CLEAR_ON_MANAGER_START}, {"GitBranch", PERSISTENT}, @@ -159,6 +158,7 @@ std::unordered_map keys = { {"UpdateFailedCount", CLEAR_ON_MANAGER_START}, {"Version", PERSISTENT}, {"VisionRadarToggle", PERSISTENT}, + {"WideCameraOnly", PERSISTENT}, {"ApiCache_Device", PERSISTENT}, {"ApiCache_DriveStats", PERSISTENT}, {"ApiCache_NavDestinations", PERSISTENT}, diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 02f1c19a77..32438fa799 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -18,7 +18,7 @@ def plannerd_thread(sm=None, pm=None): cloudlog.info("plannerd got CarParams: %s", CP.carName) use_lanelines = not params.get_bool('EndToEndToggle') - wide_camera = params.get_bool('EnableWideCamera') if TICI else False + wide_camera = params.get_bool('WideCameraOnly') cloudlog.event("e2e mode", on=use_lanelines) diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 6a8707a6cd..d7bf36fb26 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -20,7 +20,6 @@ from common.realtime import set_realtime_priority from common.transformations.model import model_height from common.transformations.camera import get_view_frame_from_road_frame from common.transformations.orientation import rot_from_euler, euler_from_rot -from selfdrive.hardware import TICI from selfdrive.swaglog import cloudlog MIN_SPEED_FILTER = 15 * CV.MPH_TO_MS @@ -68,7 +67,7 @@ class Calibrator: # Read saved calibration params = Params() calibration_params = params.get("CalibrationParams") - self.wide_camera = TICI and params.get_bool('EnableWideCamera') + self.wide_camera = params.get_bool('WideCameraOnly') rpy_init = RPY_INIT valid_blocks = 0 diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index 03d0ec4325..593abbc9e4 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -171,7 +171,7 @@ int main(int argc, char **argv) { assert(ret == 0); } - bool main_wide_camera = Params().getBool("EnableWideCamera"); + bool main_wide_camera = Params().getBool("WideCameraOnly"); bool use_extra_client = !main_wide_camera; // set for single camera mode // cl init diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index c1428ccf96..0690f1d8cf 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -350,7 +350,7 @@ def setup_env(simulation=False): params.put_bool("OpenpilotEnabledToggle", True) params.put_bool("Passive", False) params.put_bool("DisengageOnAccelerator", True) - params.put_bool("EnableWideCamera", False) + params.put_bool("WideCameraOnly", False) params.put_bool("DisableLogging", False) os.environ["NO_RADAR_SLEEP"] = "1" diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index bedb570235..31d94c2c65 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -91,7 +91,7 @@ void OnroadWindow::offroadTransition(bool offroad) { alerts->updateAlert({}, bg); // update stream type - bool wide_cam = Hardware::TICI() && Params().getBool("EnableWideCamera"); + bool wide_cam = Params().getBool("WideCameraOnly"); nvg->setStreamType(wide_cam ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD); } diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index b31e84905d..cd9bacf725 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -222,7 +222,7 @@ void UIState::updateStatus() { status = STATUS_DISENGAGED; scene.started_frame = sm->frame; scene.end_to_end = Params().getBool("EndToEndToggle"); - wide_camera = Hardware::TICI() ? Params().getBool("EnableWideCamera") : false; + wide_camera = Params().getBool("WideCameraOnly"); } started_prev = scene.started; emit offroadTransition(!scene.started); @@ -237,7 +237,7 @@ UIState::UIState(QObject *parent) : QObject(parent) { }); Params params; - wide_camera = Hardware::TICI() ? params.getBool("EnableWideCamera") : false; + wide_camera = params.getBool("WideCameraOnly"); prime_type = std::atoi(params.get("PrimeType").c_str()); // update timer diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py index dbf8f578fd..ae65e8deb8 100755 --- a/tools/sim/bridge.py +++ b/tools/sim/bridge.py @@ -36,6 +36,7 @@ def parse_args(add_args=None): parser = argparse.ArgumentParser(description='Bridge between CARLA and openpilot.') parser.add_argument('--joystick', action='store_true') parser.add_argument('--high_quality', action='store_true') + parser.add_argument('--dual_camera', action='store_true') parser.add_argument('--town', type=str, default='Town04_Opt') parser.add_argument('--spawn_point', dest='num_selected_spawn_point', type=int, default=16) @@ -112,7 +113,7 @@ class Camerad: dat = messaging.new_message(pub_type) msg = { - "frameId": image.frame, + "frameId": frame_id, "transform": [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] @@ -239,6 +240,7 @@ class CarlaBridge: msg.liveCalibration.validBlocks = 20 msg.liveCalibration.rpyCalib = [0.0, 0.0, 0.0] Params().put("CalibrationParams", msg.to_bytes()) + Params().put_bool("WideCameraOnly", not arguments.dual_camera) self._args = arguments self._carla_objects = [] @@ -333,10 +335,13 @@ class CarlaBridge: return camera self._camerad = Camerad() - road_camera = create_camera(fov=40, callback=self._camerad.cam_callback_road) - road_wide_camera = create_camera(fov=120, callback=self._camerad.cam_callback_wide_road) # fov bigger than 120 shows unwanted artifacts - self._carla_objects.extend([road_camera, road_wide_camera]) + if self._args.dual_camera: + road_camera = create_camera(fov=40, callback=self._camerad.cam_callback_road) + self._carla_objects.append(road_camera) + + road_wide_camera = create_camera(fov=120, callback=self._camerad.cam_callback_wide_road) # fov bigger than 120 shows unwanted artifacts + self._carla_objects.append(road_wide_camera) vehicle_state = VehicleState() @@ -504,6 +509,7 @@ class CarlaBridge: def close(self): self.started = False self._exit_event.set() + for s in self._carla_objects: try: s.destroy() @@ -522,17 +528,23 @@ if __name__ == "__main__": q: Any = Queue() args = parse_args() - carla_bridge = CarlaBridge(args) - p = carla_bridge.run(q) + try: + carla_bridge = CarlaBridge(args) + p = carla_bridge.run(q) - if args.joystick: - # start input poll for joystick - from tools.sim.lib.manual_ctrl import wheel_poll_thread + if args.joystick: + # start input poll for joystick + from tools.sim.lib.manual_ctrl import wheel_poll_thread - wheel_poll_thread(q) - else: - # start input poll for keyboard - from tools.sim.lib.keyboard_ctrl import keyboard_poll_thread + wheel_poll_thread(q) + else: + # start input poll for keyboard + from tools.sim.lib.keyboard_ctrl import keyboard_poll_thread + + keyboard_poll_thread(q) + p.join() - keyboard_poll_thread(q) - p.join() + finally: + # Try cleaning up the wide camera param + # in case users want to use replay after + Params().delete("WideCameraOnly") From 6bd14b86982f9a2b191b9bd5d4b82035d64d64bc Mon Sep 17 00:00:00 2001 From: AlexandreSato <66435071+AlexandreSato@users.noreply.github.com> Date: Tue, 31 May 2022 14:38:10 -0300 Subject: [PATCH 12/29] Toyota: add missing engine FW version for 2022 Corolla (#24682) From dongle id 6444d42c9eba0145 | Test route: 6444d42c9eba0145|2022-05-29--10-44-07--0 | User: geraldoy#7013 --- selfdrive/car/toyota/values.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 22089f4b6c..d201decb53 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -675,6 +675,7 @@ FW_VERSIONS = { b'\x01896630ZP1000\x00\x00\x00\x00', b'\x01896630ZP2000\x00\x00\x00\x00', b'\x01896630ZQ5000\x00\x00\x00\x00', + b'\x01896630ZU9000\x00\x00\x00\x00', b'\x018966312L8000\x00\x00\x00\x00', b'\x018966312M0000\x00\x00\x00\x00', b'\x018966312M9000\x00\x00\x00\x00', From dde05d53450f938ed62c32f7151664b9ccf67924 Mon Sep 17 00:00:00 2001 From: Erich Moraga <33645296+ErichMoraga@users.noreply.github.com> Date: Tue, 31 May 2022 15:47:08 -0500 Subject: [PATCH 13/29] Toyota: Add missing HIGHLANDER_TSS2 FW versions (#24684) `@HEADMAN#4924` 2022 Highlander Platinum (ICE) DongleID/route 427976ba75953bf3|2022-05-30--16-28-27 --- selfdrive/car/toyota/values.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index d201decb53..0b6948cf44 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -913,6 +913,7 @@ FW_VERSIONS = { b'\x01F15260E051\x00\x00\x00\x00\x00\x00', b'\x01F15260E061\x00\x00\x00\x00\x00\x00', b'\x01F15260E110\x00\x00\x00\x00\x00\x00', + b'\x01F15260E170\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x700, None): [ b'\x01896630E62100\x00\x00\x00\x00', @@ -932,10 +933,12 @@ FW_VERSIONS = { (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301400\x00\x00\x00\x00', b'\x018821F6201200\x00\x00\x00\x00', + b'\x018821F6201300\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ b'\x028646F0E02100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', b'\x028646F4803000\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', + b'\x028646F4803000\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', ], }, CAR.HIGHLANDERH_TSS2: { From 8be28b777ad5c0c7a4da6a021787e3bb4ca0f898 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Tue, 31 May 2022 14:04:10 -0700 Subject: [PATCH 14/29] add some release notes --- RELEASES.md | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/RELEASES.md b/RELEASES.md index 0ef0cd5499..c02a54413c 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,5 +1,10 @@ -Version 0.8.15 (20XX-XX-XX) +Version 0.8.15 (2022-XX-XX) ======================== +* New driving model +* New lateral controller based on physical wheel torque model + * Much smoother control, consistent across the speed range + * Effective feedforward that uses road roll + * Simplified tuning, all car-specific parameters can be derived from data Version 0.8.14 (2022-06-01) ======================== From 80be66484f4af84bd315f3e64aec22c2f74eb2fc Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 31 May 2022 22:10:11 -0700 Subject: [PATCH 15/29] fingerprinting: bump CAN fingerprinting time and drain socket (#24694) * add debugging script * debug * remove * add comment * timestamp CAN fingerprinting (to get time needed to wait for radar) * comment * need this to log * just in case, log first can packet logMonoTime * clean up * clean up * bump fingerprint time to 250ms bump fingerprint time to 250ms * comment --- selfdrive/car/car_helpers.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 46991bb4d2..85fcfa1fad 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -122,10 +122,13 @@ def fingerprint(logcan, sendcan): finger = gen_empty_fingerprint() candidate_cars = {i: all_legacy_fingerprint_cars() for i in [0, 1]} # attempt fingerprint on both bus 0 and 1 frame = 0 - frame_fingerprint = 10 # 0.1s + frame_fingerprint = 25 # 0.25s car_fingerprint = None done = False + # drain CAN socket so we always get the latest messages + messaging.drain_sock_raw(logcan) + while not done: a = get_one_can(logcan) From 338a3babd48ee7cab49791950c837951dd3084df Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 1 Jun 2022 11:29:05 +0200 Subject: [PATCH 16/29] navd: handle exceptions from requests (#24697) --- selfdrive/navd/navd.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index e063773d52..a553e5187e 100755 --- a/selfdrive/navd/navd.py +++ b/selfdrive/navd/navd.py @@ -118,9 +118,10 @@ class RouteEngine: params['bearings'] = f"{(self.last_bearing + 360) % 360:.0f},90;" url = self.mapbox_host + f'/directions/v5/mapbox/driving-traffic/{self.last_position.longitude},{self.last_position.latitude};{destination.longitude},{destination.latitude}' - resp = requests.get(url, params=params) + try: + resp = requests.get(url, params=params) + resp.raise_for_status() - if resp.status_code == 200: r = resp.json() if len(r['routes']): self.route = r['routes'][0]['legs'][0]['steps'] @@ -135,8 +136,8 @@ class RouteEngine: cloudlog.warning("Got empty route response") self.clear_route() - else: - cloudlog.warning(f"Got error in route reply {resp.status_code}") + except requests.exceptions.RequestException: + cloudlog.exception("failed to get route") self.clear_route() self.send_route() From 2b9f8662ccce470a145a12d6d3b8d998a4f77b9f Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 1 Jun 2022 14:57:58 +0200 Subject: [PATCH 17/29] navd: dont crash on unregistered device (#24699) --- selfdrive/navd/navd.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index a553e5187e..0a8eab7e06 100755 --- a/selfdrive/navd/navd.py +++ b/selfdrive/navd/navd.py @@ -47,7 +47,11 @@ class RouteEngine: self.mapbox_token = os.environ["MAPBOX_TOKEN"] self.mapbox_host = "https://api.mapbox.com" else: - self.mapbox_token = Api(self.params.get("DongleId", encoding='utf8')).get_token(expiry_hours=4 * 7 * 24) + try: + self.mapbox_token = Api(self.params.get("DongleId", encoding='utf8')).get_token(expiry_hours=4 * 7 * 24) + except FileNotFoundError: + cloudlog.exception("Failed to generate mapbox token due to missing private key. Ensure device is registered.") + self.mapbox_token = "" self.mapbox_host = "https://maps.comma.ai" def update(self): From ea5b8cdfb10686c972d8d2bc1778e6e6567e45a9 Mon Sep 17 00:00:00 2001 From: George Hotz <72895+geohot@users.noreply.github.com> Date: Wed, 1 Jun 2022 08:18:28 -0700 Subject: [PATCH 18/29] nv12: encoderd avoids a full frame copy (#24519) * rgb to nv12 * nv12 works (w memcpy) * correct now * no copy * fix nv12 with fast debayer * reverts of unused stuff * ui use nv12 * comment out thumbnails for now * rebase fix * dm read nv12 * model read nv12 * fix ffmpeg encoder * thumbnails from nv12 * replay to nv12 * python framereader support nv12 * remove hardcoded frame/buffer sizes * fix build * ffmpeg encoder fix buffers * small cleanup * reduce power usage test * fix cpu usage test * fix snapshot * fix loggerd test * bump cereal Co-authored-by: Comma Device Co-authored-by: Joost Wooning --- cereal | 2 +- selfdrive/camerad/cameras/camera_common.cc | 43 ++++++++---- selfdrive/camerad/cameras/real_debayer.cl | 13 ++-- selfdrive/camerad/snapshot/snapshot.py | 12 ++-- selfdrive/hardware/tici/test_power_draw.py | 2 +- selfdrive/loggerd/encoder/encoder.h | 3 +- selfdrive/loggerd/encoder/ffmpeg_encoder.cc | 31 ++++++--- selfdrive/loggerd/encoder/ffmpeg_encoder.h | 4 +- selfdrive/loggerd/encoder/v4l_encoder.cc | 35 ++-------- selfdrive/loggerd/encoder/v4l_encoder.h | 3 +- selfdrive/loggerd/encoderd.cc | 3 +- selfdrive/loggerd/tests/test_loggerd.py | 20 ++++-- selfdrive/modeld/dmonitoringmodeld.cc | 2 +- selfdrive/modeld/models/commonmodel.cc | 4 +- selfdrive/modeld/models/commonmodel.h | 2 +- selfdrive/modeld/models/dmonitoring.cc | 21 +++--- selfdrive/modeld/models/dmonitoring.h | 2 +- selfdrive/modeld/models/driving.cc | 4 +- selfdrive/modeld/transforms/transform.cc | 58 +++++++++-------- selfdrive/modeld/transforms/transform.cl | 14 ++-- selfdrive/modeld/transforms/transform.h | 2 +- selfdrive/test/process_replay/model_replay.py | 2 +- selfdrive/test/process_replay/regen.py | 2 +- selfdrive/test/test_onroad.py | 4 +- selfdrive/ui/qt/widgets/cameraview.cc | 65 ++++++++++--------- selfdrive/ui/qt/widgets/cameraview.h | 1 + selfdrive/ui/replay/framereader.cc | 25 +++---- tools/lib/framereader.py | 42 ++++++++++-- 28 files changed, 235 insertions(+), 186 deletions(-) diff --git a/cereal b/cereal index 93a2560618..bf4960f83c 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 93a2560618ef41bc256f7a0331c8b047863d4d69 +Subproject commit bf4960f83ccdefc7e9d2e405878ae6c8efced83b diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc index d3fd34f6e4..40a3b86cdd 100644 --- a/selfdrive/camerad/cameras/camera_common.cc +++ b/selfdrive/camerad/cameras/camera_common.cc @@ -16,6 +16,7 @@ #include "common/swaglog.h" #include "common/util.h" #include "selfdrive/hardware/hw.h" +#include "msm_media_info.h" #ifdef QCOM2 #include "CL/cl_ext_qcom.h" @@ -28,17 +29,17 @@ ExitHandler do_exit; class Debayer { public: - Debayer(cl_device_id device_id, cl_context context, const CameraBuf *b, const CameraState *s) { + Debayer(cl_device_id device_id, cl_context context, const CameraBuf *b, const CameraState *s, int buf_width, int uv_offset) { char args[4096]; const CameraInfo *ci = &s->ci; hdr_ = ci->hdr; snprintf(args, sizeof(args), "-cl-fast-relaxed-math -cl-denorms-are-zero " "-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d -DFRAME_OFFSET=%d " - "-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DRGB_STRIDE=%d " + "-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DRGB_STRIDE=%d -DYUV_STRIDE=%d -DUV_OFFSET=%d " "-DBAYER_FLIP=%d -DHDR=%d -DCAM_NUM=%d%s", ci->frame_width, ci->frame_height, ci->frame_stride, ci->frame_offset, - b->rgb_width, b->rgb_height, b->rgb_stride, + b->rgb_width, b->rgb_height, b->rgb_stride, buf_width, uv_offset, ci->bayer_flip, ci->hdr, s->camera_num, s->camera_num==1 ? " -DVIGNETTING" : ""); const char *cl_file = "cameras/real_debayer.cl"; cl_program prg_debayer = cl_program_from_file(context, device_id, cl_file, args); @@ -101,11 +102,17 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, rgb_stride = vipc_server->get_buffer(rgb_type)->stride; LOGD("created %d UI vipc buffers with size %dx%d", UI_BUF_COUNT, rgb_width, rgb_height); - vipc_server->create_buffers(yuv_type, YUV_BUFFER_COUNT, false, rgb_width, rgb_height); - LOGD("created %d YUV vipc buffers with size %dx%d", YUV_BUFFER_COUNT, rgb_width, rgb_height); + int nv12_width = VENUS_Y_STRIDE(COLOR_FMT_NV12, rgb_width); + int nv12_height = VENUS_Y_SCANLINES(COLOR_FMT_NV12, rgb_height); + assert(nv12_width == VENUS_UV_STRIDE(COLOR_FMT_NV12, rgb_width)); + assert(nv12_height/2 == VENUS_UV_SCANLINES(COLOR_FMT_NV12, rgb_height)); + size_t nv12_size = 2346 * nv12_width; // comes from v4l2_format.fmt.pix_mp.plane_fmt[0].sizeimage + size_t nv12_uv_offset = nv12_width * nv12_height; + vipc_server->create_buffers_with_sizes(yuv_type, YUV_BUFFER_COUNT, false, rgb_width, rgb_height, nv12_size, nv12_width, nv12_uv_offset); + LOGD("created %d YUV vipc buffers with size %dx%d", YUV_BUFFER_COUNT, nv12_width, nv12_height); if (ci->bayer) { - debayer = new Debayer(device_id, context, this, s); + debayer = new Debayer(device_id, context, this, s, nv12_width, nv12_uv_offset); } rgb2yuv = std::make_unique(context, device_id, rgb_width, rgb_height, rgb_stride); @@ -233,20 +240,28 @@ kj::Array get_raw_frame_image(const CameraBuf *b) { } static kj::Array yuv420_to_jpeg(const CameraBuf *b, int thumbnail_width, int thumbnail_height) { + int downscale = b->cur_yuv_buf->width / thumbnail_width; + assert(downscale * thumbnail_height == b->cur_yuv_buf->height); + int in_stride = b->cur_yuv_buf->stride; + // make the buffer big enough. jpeg_write_raw_data requires 16-pixels aligned height to be used. std::unique_ptr buf(new uint8_t[(thumbnail_width * ((thumbnail_height + 15) & ~15) * 3) / 2]); uint8_t *y_plane = buf.get(); uint8_t *u_plane = y_plane + thumbnail_width * thumbnail_height; uint8_t *v_plane = u_plane + (thumbnail_width * thumbnail_height) / 4; { - int result = libyuv::I420Scale( - b->cur_yuv_buf->y, b->rgb_width, b->cur_yuv_buf->u, b->rgb_width / 2, b->cur_yuv_buf->v, b->rgb_width / 2, - b->rgb_width, b->rgb_height, - y_plane, thumbnail_width, u_plane, thumbnail_width / 2, v_plane, thumbnail_width / 2, - thumbnail_width, thumbnail_height, libyuv::kFilterNone); - if (result != 0) { - LOGE("Generate YUV thumbnail failed."); - return {}; + // subsampled conversion from nv12 to yuv + for (int hy = 0; hy < thumbnail_height/2; hy++) { + for (int hx = 0; hx < thumbnail_width/2; hx++) { + int ix = hx * downscale + (downscale-1)/2; + int iy = hy * downscale + (downscale-1)/2; + y_plane[(hy*2 + 0)*thumbnail_width + (hx*2 + 0)] = b->cur_yuv_buf->y[(iy*2 + 0) * in_stride + ix*2 + 0]; + y_plane[(hy*2 + 0)*thumbnail_width + (hx*2 + 1)] = b->cur_yuv_buf->y[(iy*2 + 0) * in_stride + ix*2 + 1]; + y_plane[(hy*2 + 1)*thumbnail_width + (hx*2 + 0)] = b->cur_yuv_buf->y[(iy*2 + 1) * in_stride + ix*2 + 0]; + y_plane[(hy*2 + 1)*thumbnail_width + (hx*2 + 1)] = b->cur_yuv_buf->y[(iy*2 + 1) * in_stride + ix*2 + 1]; + u_plane[hy*thumbnail_width/2 + hx] = b->cur_yuv_buf->uv[iy*in_stride + ix*2 + 0]; + v_plane[hy*thumbnail_width/2 + hx] = b->cur_yuv_buf->uv[iy*in_stride + ix*2 + 1]; + } } } diff --git a/selfdrive/camerad/cameras/real_debayer.cl b/selfdrive/camerad/cameras/real_debayer.cl index dc6044ed53..fc9f410494 100644 --- a/selfdrive/camerad/cameras/real_debayer.cl +++ b/selfdrive/camerad/cameras/real_debayer.cl @@ -1,7 +1,5 @@ #define UV_WIDTH RGB_WIDTH / 2 #define UV_HEIGHT RGB_HEIGHT / 2 -#define U_OFFSET RGB_WIDTH * RGB_HEIGHT -#define V_OFFSET RGB_WIDTH * RGB_HEIGHT + UV_WIDTH * UV_HEIGHT #define RGB_TO_Y(r, g, b) ((((mul24(b, 13) + mul24(g, 65) + mul24(r, 33)) + 64) >> 7) + 16) #define RGB_TO_U(r, g, b) ((mul24(b, 56) - mul24(g, 37) - mul24(r, 19) + 0x8080) >> 8) @@ -141,17 +139,20 @@ __kernel void debayer10(const __global uchar * in, __global uchar * out) RGB_TO_Y(rgb_out[0].s0, rgb_out[0].s1, rgb_out[0].s2), RGB_TO_Y(rgb_out[1].s0, rgb_out[1].s1, rgb_out[1].s2) ); - vstore2(yy, 0, out + mad24(gid_y * 2, RGB_WIDTH, gid_x * 2)); + vstore2(yy, 0, out + mad24(gid_y * 2, YUV_STRIDE, gid_x * 2)); yy = (uchar2)( RGB_TO_Y(rgb_out[2].s0, rgb_out[2].s1, rgb_out[2].s2), RGB_TO_Y(rgb_out[3].s0, rgb_out[3].s1, rgb_out[3].s2) ); - vstore2(yy, 0, out + mad24(gid_y * 2 + 1, RGB_WIDTH, gid_x * 2)); + vstore2(yy, 0, out + mad24(gid_y * 2 + 1, YUV_STRIDE, gid_x * 2)); // write uvs const short ar = AVERAGE(rgb_out[0].s0, rgb_out[1].s0, rgb_out[2].s0, rgb_out[3].s0); const short ag = AVERAGE(rgb_out[0].s1, rgb_out[1].s1, rgb_out[2].s1, rgb_out[3].s1); const short ab = AVERAGE(rgb_out[0].s2, rgb_out[1].s2, rgb_out[2].s2, rgb_out[3].s2); - out[U_OFFSET + mad24(gid_y, UV_WIDTH, gid_x)] = RGB_TO_U(ar, ag, ab); - out[V_OFFSET + mad24(gid_y, UV_WIDTH, gid_x)] = RGB_TO_V(ar, ag, ab); + uchar2 uv = (uchar2)( + RGB_TO_U(ar, ag, ab), + RGB_TO_V(ar, ag, ab) + ); + vstore2(uv, 0, out + UV_OFFSET + mad24(gid_y, YUV_STRIDE, gid_x * 2)); } diff --git a/selfdrive/camerad/snapshot/snapshot.py b/selfdrive/camerad/snapshot/snapshot.py index db1342ee7d..bf37ca181e 100755 --- a/selfdrive/camerad/snapshot/snapshot.py +++ b/selfdrive/camerad/snapshot/snapshot.py @@ -44,10 +44,10 @@ def yuv_to_rgb(y, u, v): return rgb.astype(np.uint8) -def extract_image(buf, w, h): - y = np.array(buf[:w*h], dtype=np.uint8).reshape((h, w)) - u = np.array(buf[w*h: w*h+(w//2)*(h//2)], dtype=np.uint8).reshape((h//2, w//2)) - v = np.array(buf[w*h+(w//2)*(h//2):], dtype=np.uint8).reshape((h//2, w//2)) +def extract_image(buf, w, h, stride, uv_offset): + y = np.array(buf[:uv_offset], dtype=np.uint8).reshape((-1, stride))[:h, :w] + u = np.array(buf[uv_offset::2], dtype=np.uint8).reshape((-1, stride//2))[:h//2, :w//2] + v = np.array(buf[uv_offset+1::2], dtype=np.uint8).reshape((-1, stride//2))[:h//2, :w//2] return yuv_to_rgb(y, u, v) @@ -79,10 +79,10 @@ def get_snapshots(frame="roadCameraState", front_frame="driverCameraState", focu rear, front = None, None if frame is not None: c = vipc_clients[frame] - rear = extract_image(c.recv(), c.width, c.height) + rear = extract_image(c.recv(), c.width, c.height, c.stride, c.uv_offset) if front_frame is not None: c = vipc_clients[front_frame] - front = extract_image(c.recv(), c.width, c.height) + front = extract_image(c.recv(), c.width, c.height, c.stride, c.uv_offset) return rear, front diff --git a/selfdrive/hardware/tici/test_power_draw.py b/selfdrive/hardware/tici/test_power_draw.py index d6af25a4e5..934d72e3a1 100755 --- a/selfdrive/hardware/tici/test_power_draw.py +++ b/selfdrive/hardware/tici/test_power_draw.py @@ -22,7 +22,7 @@ PROCS = [ Proc('camerad', 2.25), Proc('modeld', 0.95), Proc('dmonitoringmodeld', 0.25), - Proc('encoderd', 0.42), + Proc('encoderd', 0.23), ] diff --git a/selfdrive/loggerd/encoder/encoder.h b/selfdrive/loggerd/encoder/encoder.h index 9792d61c46..312b68ba19 100644 --- a/selfdrive/loggerd/encoder/encoder.h +++ b/selfdrive/loggerd/encoder/encoder.h @@ -19,8 +19,7 @@ public: : filename(filename), type(type), in_width(in_width), in_height(in_height), fps(fps), bitrate(bitrate), codec(codec), out_width(out_width), out_height(out_height), write(write) { } virtual ~VideoEncoder(); - virtual int encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, - int in_width, int in_height, VisionIpcBufExtra *extra) = 0; + virtual int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) = 0; virtual void encoder_open(const char* path) = 0; virtual void encoder_close() = 0; diff --git a/selfdrive/loggerd/encoder/ffmpeg_encoder.cc b/selfdrive/loggerd/encoder/ffmpeg_encoder.cc index a2b14ef3fd..22587819a4 100644 --- a/selfdrive/loggerd/encoder/ffmpeg_encoder.cc +++ b/selfdrive/loggerd/encoder/ffmpeg_encoder.cc @@ -34,6 +34,8 @@ void FfmpegEncoder::encoder_init() { frame->linesize[1] = out_width/2; frame->linesize[2] = out_width/2; + convert_buf.resize(in_width * in_height * 3 / 2); + if (in_width != out_width || in_height != out_height) { downscale_buf.resize(out_width * out_height * 3 / 2); } @@ -70,18 +72,27 @@ void FfmpegEncoder::encoder_close() { is_open = false; } -int FfmpegEncoder::encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, - int in_width_, int in_height_, VisionIpcBufExtra *extra) { - assert(in_width_ == this->in_width); - assert(in_height_ == this->in_height); +int FfmpegEncoder::encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) { + assert(buf->width == this->in_width); + assert(buf->height == this->in_height); + + uint8_t *cy = convert_buf.data(); + uint8_t *cu = cy + in_width * in_height; + uint8_t *cv = cu + (in_width / 2) * (in_height / 2); + libyuv::NV12ToI420(buf->y, buf->stride, + buf->uv, buf->stride, + cy, in_width, + cu, in_width/2, + cv, in_width/2, + in_width, in_height); if (downscale_buf.size() > 0) { uint8_t *out_y = downscale_buf.data(); uint8_t *out_u = out_y + frame->width * frame->height; uint8_t *out_v = out_u + (frame->width / 2) * (frame->height / 2); - libyuv::I420Scale(y_ptr, in_width, - u_ptr, in_width/2, - v_ptr, in_width/2, + libyuv::I420Scale(cy, in_width, + cu, in_width/2, + cv, in_width/2, in_width, in_height, out_y, frame->width, out_u, frame->width/2, @@ -92,9 +103,9 @@ int FfmpegEncoder::encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, cons frame->data[1] = out_u; frame->data[2] = out_v; } else { - frame->data[0] = (uint8_t*)y_ptr; - frame->data[1] = (uint8_t*)u_ptr; - frame->data[2] = (uint8_t*)v_ptr; + frame->data[0] = cy; + frame->data[1] = cu; + frame->data[2] = cv; } frame->pts = counter*50*1000; // 50ms per frame diff --git a/selfdrive/loggerd/encoder/ffmpeg_encoder.h b/selfdrive/loggerd/encoder/ffmpeg_encoder.h index ae41abc69f..497a28b651 100644 --- a/selfdrive/loggerd/encoder/ffmpeg_encoder.h +++ b/selfdrive/loggerd/encoder/ffmpeg_encoder.h @@ -21,8 +21,7 @@ class FfmpegEncoder : public VideoEncoder { VideoEncoder(filename, type, in_width, in_height, fps, bitrate, cereal::EncodeIndex::Type::BIG_BOX_LOSSLESS, out_width, out_height, write) { encoder_init(); } ~FfmpegEncoder(); void encoder_init(); - int encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, - int in_width_, int in_height_, VisionIpcBufExtra *extra); + int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra); void encoder_open(const char* path); void encoder_close(); @@ -33,5 +32,6 @@ private: AVCodecContext *codec_ctx; AVFrame *frame = NULL; + std::vector convert_buf; std::vector downscale_buf; }; diff --git a/selfdrive/loggerd/encoder/v4l_encoder.cc b/selfdrive/loggerd/encoder/v4l_encoder.cc index 5fb9cc8de7..b3bd692b16 100644 --- a/selfdrive/loggerd/encoder/v4l_encoder.cc +++ b/selfdrive/loggerd/encoder/v4l_encoder.cc @@ -248,7 +248,6 @@ void V4LEncoder::encoder_init() { } // queue up input buffers for (unsigned int i = 0; i < BUF_IN_COUNT; i++) { - buf_in[i].allocate(fmt_in.fmt.pix_mp.plane_fmt[0].sizeimage); free_buf_in.push(i); } @@ -262,41 +261,19 @@ void V4LEncoder::encoder_open(const char* path) { this->counter = 0; } -int V4LEncoder::encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, - int in_width_, int in_height_, VisionIpcBufExtra *extra) { - assert(in_width == in_width_); - assert(in_height == in_height_); - assert(is_open); - - // reserve buffer - int buffer_in = free_buf_in.pop(); - - uint8_t *in_y_ptr = (uint8_t*)buf_in[buffer_in].addr; - int in_y_stride = VENUS_Y_STRIDE(COLOR_FMT_NV12, in_width); - int in_uv_stride = VENUS_UV_STRIDE(COLOR_FMT_NV12, in_width); - uint8_t *in_uv_ptr = in_y_ptr + (in_y_stride * VENUS_Y_SCANLINES(COLOR_FMT_NV12, in_height)); - - - // GRRR COPY - int err = libyuv::I420ToNV12(y_ptr, in_width, - u_ptr, in_width/2, - v_ptr, in_width/2, - in_y_ptr, in_y_stride, - in_uv_ptr, in_uv_stride, - in_width, in_height); - assert(err == 0); - +int V4LEncoder::encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) { struct timeval timestamp { .tv_sec = (long)(extra->timestamp_eof/1000000000), .tv_usec = (long)((extra->timestamp_eof/1000) % 1000000), }; + // reserve buffer + int buffer_in = free_buf_in.pop(); + // push buffer extras.push(*extra); - buf_in[buffer_in].sync(VISIONBUF_SYNC_TO_DEVICE); - int bytesused = VENUS_Y_STRIDE(COLOR_FMT_NV12, in_width) * VENUS_Y_SCANLINES(COLOR_FMT_NV12, in_height) + - VENUS_UV_STRIDE(COLOR_FMT_NV12, in_width) * VENUS_UV_SCANLINES(COLOR_FMT_NV12, in_height); - queue_buffer(fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, buffer_in, &buf_in[buffer_in], timestamp, bytesused); + //buf->sync(VISIONBUF_SYNC_TO_DEVICE); + queue_buffer(fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, buffer_in, buf, timestamp); return this->counter++; } diff --git a/selfdrive/loggerd/encoder/v4l_encoder.h b/selfdrive/loggerd/encoder/v4l_encoder.h index 9f267cfb03..b7c378be85 100644 --- a/selfdrive/loggerd/encoder/v4l_encoder.h +++ b/selfdrive/loggerd/encoder/v4l_encoder.h @@ -13,8 +13,7 @@ public: VideoEncoder(filename, type, in_width, in_height, fps, bitrate, codec, out_width, out_height, write) { encoder_init(); } ~V4LEncoder(); void encoder_init(); - int encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, - int in_width, int in_height, VisionIpcBufExtra *extra); + int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra); void encoder_open(const char* path); void encoder_close(); private: diff --git a/selfdrive/loggerd/encoderd.cc b/selfdrive/loggerd/encoderd.cc index 297eaf88d0..5eb70dad33 100644 --- a/selfdrive/loggerd/encoderd.cc +++ b/selfdrive/loggerd/encoderd.cc @@ -103,8 +103,7 @@ void encoder_thread(EncoderdState *s, const LogCameraInfo &cam_info) { // encode a frame for (int i = 0; i < encoders.size(); ++i) { - int out_id = encoders[i]->encode_frame(buf->y, buf->u, buf->v, - buf->width, buf->height, &extra); + int out_id = encoders[i]->encode_frame(buf, &extra); if (out_id == -1) { LOGE("Failed to encode frame. frame_id: %d", extra.frame_id); diff --git a/selfdrive/loggerd/tests/test_loggerd.py b/selfdrive/loggerd/tests/test_loggerd.py index 89fb0c5838..8eecd834e1 100755 --- a/selfdrive/loggerd/tests/test_loggerd.py +++ b/selfdrive/loggerd/tests/test_loggerd.py @@ -109,16 +109,19 @@ class TestLoggerd(unittest.TestCase): Params().put("RecordFront", "1") expected_files = {"rlog", "qlog", "qcamera.ts", "fcamera.hevc", "dcamera.hevc"} - streams = [(VisionStreamType.VISION_STREAM_ROAD, tici_f_frame_size if TICI else eon_f_frame_size, "roadCameraState"), - (VisionStreamType.VISION_STREAM_DRIVER, tici_d_frame_size if TICI else eon_d_frame_size, "driverCameraState")] + streams = [(VisionStreamType.VISION_STREAM_ROAD, (*tici_f_frame_size, 2048*2346, 2048, 2048*1216) if TICI else eon_f_frame_size, "roadCameraState"), + (VisionStreamType.VISION_STREAM_DRIVER, (*tici_d_frame_size, 2048*2346, 2048, 2048*1216) if TICI else eon_d_frame_size, "driverCameraState")] if TICI: expected_files.add("ecamera.hevc") - streams.append((VisionStreamType.VISION_STREAM_WIDE_ROAD, tici_e_frame_size, "wideRoadCameraState")) + streams.append((VisionStreamType.VISION_STREAM_WIDE_ROAD, (*tici_e_frame_size, 2048*2346, 2048, 2048*1216), "wideRoadCameraState")) pm = messaging.PubMaster(["roadCameraState", "driverCameraState", "wideRoadCameraState"]) vipc_server = VisionIpcServer("camerad") - for stream_type, frame_size, _ in streams: - vipc_server.create_buffers(stream_type, 40, False, *(frame_size)) + for stream_type, frame_spec, _ in streams: + if TICI: + vipc_server.create_buffers_with_sizes(stream_type, 40, False, *(frame_spec)) + else: + vipc_server.create_buffers(stream_type, 40, False, *(frame_spec)) vipc_server.start_listener() for _ in range(5): @@ -130,8 +133,11 @@ class TestLoggerd(unittest.TestCase): fps = 20.0 for n in range(1, int(num_segs*length*fps)+1): - for stream_type, frame_size, state in streams: - dat = np.empty(int(frame_size[0]*frame_size[1]*3/2), dtype=np.uint8) + for stream_type, frame_spec, state in streams: + if TICI: + dat = np.empty(frame_spec[2], dtype=np.uint8) + else: + dat = np.empty(int(frame_spec[0]*frame_spec[1]*3/2), dtype=np.uint8) vipc_server.send(stream_type, dat[:].flatten().tobytes(), n, n/fps, n/fps) camera_state = messaging.new_message(state) diff --git a/selfdrive/modeld/dmonitoringmodeld.cc b/selfdrive/modeld/dmonitoringmodeld.cc index 61f3343c8d..68c49572e6 100644 --- a/selfdrive/modeld/dmonitoringmodeld.cc +++ b/selfdrive/modeld/dmonitoringmodeld.cc @@ -31,7 +31,7 @@ void run_model(DMonitoringModelState &model, VisionIpcClient &vipc_client) { } double t1 = millis_since_boot(); - DMonitoringResult res = dmonitoring_eval_frame(&model, buf->addr, buf->width, buf->height, calib); + DMonitoringResult res = dmonitoring_eval_frame(&model, buf->addr, buf->width, buf->height, buf->stride, buf->uv_offset, calib); double t2 = millis_since_boot(); // send dm packet diff --git a/selfdrive/modeld/models/commonmodel.cc b/selfdrive/modeld/models/commonmodel.cc index bc013395df..b7c9051c6e 100644 --- a/selfdrive/modeld/models/commonmodel.cc +++ b/selfdrive/modeld/models/commonmodel.cc @@ -22,9 +22,9 @@ ModelFrame::ModelFrame(cl_device_id device_id, cl_context context) { loadyuv_init(&loadyuv, context, device_id, MODEL_WIDTH, MODEL_HEIGHT); } -float* ModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, const mat3 &projection, cl_mem *output) { +float* ModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3 &projection, cl_mem *output) { transform_queue(&this->transform, q, - yuv_cl, frame_width, frame_height, + yuv_cl, frame_width, frame_height, frame_stride, frame_uv_offset, y_cl, u_cl, v_cl, MODEL_WIDTH, MODEL_HEIGHT, projection); if (output == NULL) { diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h index 059a85e7dd..40c82a8c21 100644 --- a/selfdrive/modeld/models/commonmodel.h +++ b/selfdrive/modeld/models/commonmodel.h @@ -25,7 +25,7 @@ class ModelFrame { public: ModelFrame(cl_device_id device_id, cl_context context); ~ModelFrame(); - float* prepare(cl_mem yuv_cl, int width, int height, const mat3& transform, cl_mem *output); + float* prepare(cl_mem yuv_cl, int width, int height, int frame_stride, int frame_uv_offset, const mat3& transform, cl_mem *output); const int MODEL_WIDTH = 512; const int MODEL_HEIGHT = 256; diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc index 4792806c8b..4cc1cd1229 100644 --- a/selfdrive/modeld/models/dmonitoring.cc +++ b/selfdrive/modeld/models/dmonitoring.cc @@ -55,19 +55,20 @@ static inline auto get_yuv_buf(std::vector &buf, const int width, int h } struct Rect {int x, y, w, h;}; -void crop_yuv(uint8_t *raw, int width, int height, uint8_t *y, uint8_t *u, uint8_t *v, const Rect &rect) { +void crop_nv12_to_yuv(uint8_t *raw, int stride, int uv_offset, uint8_t *y, uint8_t *u, uint8_t *v, const Rect &rect) { uint8_t *raw_y = raw; - uint8_t *raw_u = raw_y + (width * height); - uint8_t *raw_v = raw_u + ((width / 2) * (height / 2)); + uint8_t *raw_uv = raw_y + uv_offset; for (int r = 0; r < rect.h / 2; r++) { - memcpy(y + 2 * r * rect.w, raw_y + (2 * r + rect.y) * width + rect.x, rect.w); - memcpy(y + (2 * r + 1) * rect.w, raw_y + (2 * r + rect.y + 1) * width + rect.x, rect.w); - memcpy(u + r * (rect.w / 2), raw_u + (r + (rect.y / 2)) * width / 2 + (rect.x / 2), rect.w / 2); - memcpy(v + r * (rect.w / 2), raw_v + (r + (rect.y / 2)) * width / 2 + (rect.x / 2), rect.w / 2); + memcpy(y + 2 * r * rect.w, raw_y + (2 * r + rect.y) * stride + rect.x, rect.w); + memcpy(y + (2 * r + 1) * rect.w, raw_y + (2 * r + rect.y + 1) * stride + rect.x, rect.w); + for (int h = 0; h < rect.w / 2; h++) { + u[r * rect.w/2 + h] = raw_uv[(r + (rect.y/2)) * stride + (rect.x/2 + h)*2]; + v[r * rect.w/2 + h] = raw_uv[(r + (rect.y/2)) * stride + (rect.x/2 + h)*2 + 1]; + } } } -DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, float *calib) { +DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, int stride, int uv_offset, float *calib) { Rect crop_rect; if (width == TICI_CAM_WIDTH) { const int cropped_height = tici_dm_crop::width / 1.33; @@ -91,10 +92,10 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_ auto [cropped_y, cropped_u, cropped_v] = get_yuv_buf(s->cropped_buf, crop_rect.w, crop_rect.h); if (!s->is_rhd) { - crop_yuv((uint8_t *)stream_buf, width, height, cropped_y, cropped_u, cropped_v, crop_rect); + crop_nv12_to_yuv((uint8_t *)stream_buf, stride, uv_offset, cropped_y, cropped_u, cropped_v, crop_rect); } else { auto [mirror_y, mirror_u, mirror_v] = get_yuv_buf(s->premirror_cropped_buf, crop_rect.w, crop_rect.h); - crop_yuv((uint8_t *)stream_buf, width, height, mirror_y, mirror_u, mirror_v, crop_rect); + crop_nv12_to_yuv((uint8_t *)stream_buf, stride, uv_offset, mirror_y, mirror_u, mirror_v, crop_rect); libyuv::I420Mirror(mirror_y, crop_rect.w, mirror_u, crop_rect.w / 2, mirror_v, crop_rect.w / 2, diff --git a/selfdrive/modeld/models/dmonitoring.h b/selfdrive/modeld/models/dmonitoring.h index db4e2ef72a..a1be91e3bb 100644 --- a/selfdrive/modeld/models/dmonitoring.h +++ b/selfdrive/modeld/models/dmonitoring.h @@ -46,7 +46,7 @@ typedef struct DMonitoringModelState { } DMonitoringModelState; void dmonitoring_init(DMonitoringModelState* s); -DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, float *calib); +DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, int stride, int uv_offset, float *calib); void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, float execution_time, kj::ArrayPtr raw_pred); void dmonitoring_free(DMonitoringModelState* s); diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index b601c4898b..59c0b249d9 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -73,12 +73,12 @@ ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf, #endif // if getInputBuf is not NULL, net_input_buf will be - auto net_input_buf = s->frame->prepare(buf->buf_cl, buf->width, buf->height, transform, static_cast(s->m->getInputBuf())); + auto net_input_buf = s->frame->prepare(buf->buf_cl, buf->width, buf->height, buf->stride, buf->uv_offset, transform, static_cast(s->m->getInputBuf())); s->m->addImage(net_input_buf, s->frame->buf_size); LOGT("Image added"); if (wbuf != nullptr) { - auto net_extra_buf = s->wide_frame->prepare(wbuf->buf_cl, wbuf->width, wbuf->height, transform_wide, static_cast(s->m->getExtraBuf())); + auto net_extra_buf = s->wide_frame->prepare(wbuf->buf_cl, wbuf->width, wbuf->height, wbuf->stride, wbuf->uv_offset, transform_wide, static_cast(s->m->getExtraBuf())); s->m->addExtra(net_extra_buf, s->wide_frame->buf_size); LOGT("Extra image added"); } diff --git a/selfdrive/modeld/transforms/transform.cc b/selfdrive/modeld/transforms/transform.cc index 8929f56a9e..cabc58a46d 100644 --- a/selfdrive/modeld/transforms/transform.cc +++ b/selfdrive/modeld/transforms/transform.cc @@ -25,7 +25,7 @@ void transform_destroy(Transform* s) { void transform_queue(Transform* s, cl_command_queue q, - cl_mem in_yuv, int in_width, int in_height, + cl_mem in_yuv, int in_width, int in_height, int in_stride, int in_uv_offset, cl_mem out_y, cl_mem out_u, cl_mem out_v, int out_width, int out_height, const mat3& projection) { @@ -44,28 +44,30 @@ void transform_queue(Transform* s, const int in_y_width = in_width; const int in_y_height = in_height; + const int in_y_px_stride = 1; const int in_uv_width = in_width/2; const int in_uv_height = in_height/2; - const int in_y_offset = 0; - const int in_u_offset = in_y_offset + in_y_width*in_y_height; - const int in_v_offset = in_u_offset + in_uv_width*in_uv_height; + const int in_uv_px_stride = 2; + const int in_u_offset = in_uv_offset; + const int in_v_offset = in_uv_offset + 1; const int out_y_width = out_width; const int out_y_height = out_height; const int out_uv_width = out_width/2; const int out_uv_height = out_height/2; - CL_CHECK(clSetKernelArg(s->krnl, 0, sizeof(cl_mem), &in_yuv)); - CL_CHECK(clSetKernelArg(s->krnl, 1, sizeof(cl_int), &in_y_width)); - CL_CHECK(clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_y_offset)); - CL_CHECK(clSetKernelArg(s->krnl, 3, sizeof(cl_int), &in_y_height)); - CL_CHECK(clSetKernelArg(s->krnl, 4, sizeof(cl_int), &in_y_width)); - CL_CHECK(clSetKernelArg(s->krnl, 5, sizeof(cl_mem), &out_y)); - CL_CHECK(clSetKernelArg(s->krnl, 6, sizeof(cl_int), &out_y_width)); - CL_CHECK(clSetKernelArg(s->krnl, 7, sizeof(cl_int), &zero)); - CL_CHECK(clSetKernelArg(s->krnl, 8, sizeof(cl_int), &out_y_height)); - CL_CHECK(clSetKernelArg(s->krnl, 9, sizeof(cl_int), &out_y_width)); - CL_CHECK(clSetKernelArg(s->krnl, 10, sizeof(cl_mem), &s->m_y_cl)); + CL_CHECK(clSetKernelArg(s->krnl, 0, sizeof(cl_mem), &in_yuv)); // src + CL_CHECK(clSetKernelArg(s->krnl, 1, sizeof(cl_int), &in_stride)); // src_row_stride + CL_CHECK(clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_y_px_stride)); // src_px_stride + CL_CHECK(clSetKernelArg(s->krnl, 3, sizeof(cl_int), &zero)); // src_offset + CL_CHECK(clSetKernelArg(s->krnl, 4, sizeof(cl_int), &in_y_height)); // src_rows + CL_CHECK(clSetKernelArg(s->krnl, 5, sizeof(cl_int), &in_y_width)); // src_cols + CL_CHECK(clSetKernelArg(s->krnl, 6, sizeof(cl_mem), &out_y)); // dst + CL_CHECK(clSetKernelArg(s->krnl, 7, sizeof(cl_int), &out_y_width)); // dst_row_stride + CL_CHECK(clSetKernelArg(s->krnl, 8, sizeof(cl_int), &zero)); // dst_offset + CL_CHECK(clSetKernelArg(s->krnl, 9, sizeof(cl_int), &out_y_height)); // dst_rows + CL_CHECK(clSetKernelArg(s->krnl, 10, sizeof(cl_int), &out_y_width)); // dst_cols + CL_CHECK(clSetKernelArg(s->krnl, 11, sizeof(cl_mem), &s->m_y_cl)); // M const size_t work_size_y[2] = {(size_t)out_y_width, (size_t)out_y_height}; @@ -74,21 +76,21 @@ void transform_queue(Transform* s, const size_t work_size_uv[2] = {(size_t)out_uv_width, (size_t)out_uv_height}; - CL_CHECK(clSetKernelArg(s->krnl, 1, sizeof(cl_int), &in_uv_width)); - CL_CHECK(clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_u_offset)); - CL_CHECK(clSetKernelArg(s->krnl, 3, sizeof(cl_int), &in_uv_height)); - CL_CHECK(clSetKernelArg(s->krnl, 4, sizeof(cl_int), &in_uv_width)); - CL_CHECK(clSetKernelArg(s->krnl, 5, sizeof(cl_mem), &out_u)); - CL_CHECK(clSetKernelArg(s->krnl, 6, sizeof(cl_int), &out_uv_width)); - CL_CHECK(clSetKernelArg(s->krnl, 7, sizeof(cl_int), &zero)); - CL_CHECK(clSetKernelArg(s->krnl, 8, sizeof(cl_int), &out_uv_height)); - CL_CHECK(clSetKernelArg(s->krnl, 9, sizeof(cl_int), &out_uv_width)); - CL_CHECK(clSetKernelArg(s->krnl, 10, sizeof(cl_mem), &s->m_uv_cl)); - + CL_CHECK(clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_uv_px_stride)); // src_px_stride + CL_CHECK(clSetKernelArg(s->krnl, 3, sizeof(cl_int), &in_u_offset)); // src_offset + CL_CHECK(clSetKernelArg(s->krnl, 4, sizeof(cl_int), &in_uv_height)); // src_rows + CL_CHECK(clSetKernelArg(s->krnl, 5, sizeof(cl_int), &in_uv_width)); // src_cols + CL_CHECK(clSetKernelArg(s->krnl, 6, sizeof(cl_mem), &out_u)); // dst + CL_CHECK(clSetKernelArg(s->krnl, 7, sizeof(cl_int), &out_uv_width)); // dst_row_stride + CL_CHECK(clSetKernelArg(s->krnl, 8, sizeof(cl_int), &zero)); // dst_offset + CL_CHECK(clSetKernelArg(s->krnl, 9, sizeof(cl_int), &out_uv_height)); // dst_rows + CL_CHECK(clSetKernelArg(s->krnl, 10, sizeof(cl_int), &out_uv_width)); // dst_cols + CL_CHECK(clSetKernelArg(s->krnl, 11, sizeof(cl_mem), &s->m_uv_cl)); // M + CL_CHECK(clEnqueueNDRangeKernel(q, s->krnl, 2, NULL, (const size_t*)&work_size_uv, NULL, 0, 0, NULL)); - CL_CHECK(clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_v_offset)); - CL_CHECK(clSetKernelArg(s->krnl, 5, sizeof(cl_mem), &out_v)); + CL_CHECK(clSetKernelArg(s->krnl, 3, sizeof(cl_int), &in_v_offset)); // src_ofset + CL_CHECK(clSetKernelArg(s->krnl, 6, sizeof(cl_mem), &out_v)); // dst CL_CHECK(clEnqueueNDRangeKernel(q, s->krnl, 2, NULL, (const size_t*)&work_size_uv, NULL, 0, 0, NULL)); diff --git a/selfdrive/modeld/transforms/transform.cl b/selfdrive/modeld/transforms/transform.cl index 8ad1869351..357ef87321 100644 --- a/selfdrive/modeld/transforms/transform.cl +++ b/selfdrive/modeld/transforms/transform.cl @@ -6,9 +6,9 @@ #define INTER_REMAP_COEF_SCALE (1 << INTER_REMAP_COEF_BITS) __kernel void warpPerspective(__global const uchar * src, - int src_step, int src_offset, int src_rows, int src_cols, + int src_row_stride, int src_px_stride, int src_offset, int src_rows, int src_cols, __global uchar * dst, - int dst_step, int dst_offset, int dst_rows, int dst_cols, + int dst_row_stride, int dst_offset, int dst_rows, int dst_cols, __constant float * M) { int dx = get_global_id(0); @@ -28,18 +28,18 @@ __kernel void warpPerspective(__global const uchar * src, short ax = (short)(X & (INTER_TAB_SIZE - 1)); int v0 = (sx >= 0 && sx < src_cols && sy >= 0 && sy < src_rows) ? - convert_int(src[mad24(sy, src_step, src_offset + sx)]) : 0; + convert_int(src[mad24(sy, src_row_stride, src_offset + sx*src_px_stride)]) : 0; int v1 = (sx+1 >= 0 && sx+1 < src_cols && sy >= 0 && sy < src_rows) ? - convert_int(src[mad24(sy, src_step, src_offset + (sx+1))]) : 0; + convert_int(src[mad24(sy, src_row_stride, src_offset + (sx+1)*src_px_stride)]) : 0; int v2 = (sx >= 0 && sx < src_cols && sy+1 >= 0 && sy+1 < src_rows) ? - convert_int(src[mad24(sy+1, src_step, src_offset + sx)]) : 0; + convert_int(src[mad24(sy+1, src_row_stride, src_offset + sx*src_px_stride)]) : 0; int v3 = (sx+1 >= 0 && sx+1 < src_cols && sy+1 >= 0 && sy+1 < src_rows) ? - convert_int(src[mad24(sy+1, src_step, src_offset + (sx+1))]) : 0; + convert_int(src[mad24(sy+1, src_row_stride, src_offset + (sx+1)*src_px_stride)]) : 0; float taby = 1.f/INTER_TAB_SIZE*ay; float tabx = 1.f/INTER_TAB_SIZE*ax; - int dst_index = mad24(dy, dst_step, dst_offset + dx); + int dst_index = mad24(dy, dst_row_stride, dst_offset + dx); int itab0 = convert_short_sat_rte( (1.0f-taby)*(1.0f-tabx) * INTER_REMAP_COEF_SCALE ); int itab1 = convert_short_sat_rte( (1.0f-taby)*tabx * INTER_REMAP_COEF_SCALE ); diff --git a/selfdrive/modeld/transforms/transform.h b/selfdrive/modeld/transforms/transform.h index a1629a517e..771a7054b3 100644 --- a/selfdrive/modeld/transforms/transform.h +++ b/selfdrive/modeld/transforms/transform.h @@ -19,7 +19,7 @@ void transform_init(Transform* s, cl_context ctx, cl_device_id device_id); void transform_destroy(Transform* transform); void transform_queue(Transform* s, cl_command_queue q, - cl_mem yuv, int in_width, int in_height, + cl_mem yuv, int in_width, int in_height, int in_stride, int in_uv_offset, cl_mem out_y, cl_mem out_u, cl_mem out_v, int out_width, int out_height, const mat3& projection); diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py index 20739ae37d..34f35e0ed7 100755 --- a/selfdrive/test/process_replay/model_replay.py +++ b/selfdrive/test/process_replay/model_replay.py @@ -96,7 +96,7 @@ def model_replay(lr, frs): if msg.which() in VIPC_STREAM: msg = msg.as_builder() camera_state = getattr(msg, msg.which()) - img = frs[msg.which()].get(frame_idxs[msg.which()], pix_fmt="yuv420p")[0] + img = frs[msg.which()].get(frame_idxs[msg.which()], pix_fmt="nv12")[0] frame_idxs[msg.which()] += 1 # send camera state and frame diff --git a/selfdrive/test/process_replay/regen.py b/selfdrive/test/process_replay/regen.py index b0bc033fe6..bcc91b3ab6 100755 --- a/selfdrive/test/process_replay/regen.py +++ b/selfdrive/test/process_replay/regen.py @@ -164,7 +164,7 @@ def replay_cameras(lr, frs): print(f"Decompressing frames {s}") frames = [] for i in tqdm(range(fr.frame_count)): - img = fr.get(i, pix_fmt='yuv420p')[0] + img = fr.get(i, pix_fmt='nv12')[0] frames.append(img.flatten().tobytes()) vs.create_buffers(stream, 40, False, size[0], size[1]) diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 725b0dd1ea..6cf53a046e 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -23,11 +23,11 @@ from tools.lib.logreader import LogReader PROCS = { "selfdrive.controls.controlsd": 35.0, "./loggerd": 10.0, - "./encoderd": 37.3, + "./encoderd": 12.5, "./camerad": 16.5, "./locationd": 9.1, "selfdrive.controls.plannerd": 11.7, - "./_ui": 21.0, + "./_ui": 26.4, "selfdrive.locationd.paramsd": 9.0, "./_sensord": 6.17, "selfdrive.controls.radard": 4.5, diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index a0b3c43cde..eade9cbe30 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -34,17 +34,15 @@ const char frame_fragment_shader[] = "precision mediump float;\n" #endif "uniform sampler2D uTextureY;\n" - "uniform sampler2D uTextureU;\n" - "uniform sampler2D uTextureV;\n" + "uniform sampler2D uTextureUV;\n" "in vec2 vTexCoord;\n" "out vec4 colorOut;\n" "void main() {\n" " float y = texture(uTextureY, vTexCoord).r;\n" - " float u = texture(uTextureU, vTexCoord).r - 0.5;\n" - " float v = texture(uTextureV, vTexCoord).r - 0.5;\n" - " float r = y + 1.402 * v;\n" - " float g = y - 0.344 * u - 0.714 * v;\n" - " float b = y + 1.772 * u;\n" + " vec2 uv = texture(uTextureUV, vTexCoord).rg - 0.5;\n" + " float r = y + 1.402 * uv.y;\n" + " float g = y - 0.344 * uv.x - 0.714 * uv.y;\n" + " float b = y + 1.772 * uv.x;\n" " colorOut = vec4(r, g, b, 1.0);\n" "}\n"; @@ -158,8 +156,7 @@ void CameraViewWidget::initializeGL() { glGenTextures(3, textures); glUseProgram(program->programId()); glUniform1i(program->uniformLocation("uTextureY"), 0); - glUniform1i(program->uniformLocation("uTextureU"), 1); - glUniform1i(program->uniformLocation("uTextureV"), 2); + glUniform1i(program->uniformLocation("uTextureUV"), 1); } void CameraViewWidget::showEvent(QShowEvent *event) { @@ -223,19 +220,23 @@ void CameraViewWidget::paintGL() { VisionBuf *frame = frames[frame_idx].second; glPixelStorei(GL_UNPACK_ALIGNMENT, 1); + glPixelStorei(GL_UNPACK_ROW_LENGTH, stream_stride); glViewport(0, 0, width(), height()); glBindVertexArray(frame_vao); glUseProgram(program->programId()); - uint8_t *address[3] = {frame->y, frame->u, frame->v}; - for (int i = 0; i < 3; ++i) { - glActiveTexture(GL_TEXTURE0 + i); - glBindTexture(GL_TEXTURE_2D, textures[i]); - int width = i == 0 ? stream_width : stream_width / 2; - int height = i == 0 ? stream_height : stream_height / 2; - glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, width, height, GL_RED, GL_UNSIGNED_BYTE, address[i]); - assert(glGetError() == GL_NO_ERROR); - } + + glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_2D, textures[0]); + 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]); + 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); glUniformMatrix4fv(program->uniformLocation("uTransform"), 1, GL_TRUE, frame_mat.v); assert(glGetError() == GL_NO_ERROR); @@ -246,6 +247,7 @@ void CameraViewWidget::paintGL() { glBindTexture(GL_TEXTURE_2D, 0); glActiveTexture(GL_TEXTURE0); glPixelStorei(GL_UNPACK_ALIGNMENT, 4); + glPixelStorei(GL_UNPACK_ROW_LENGTH, 0); } void CameraViewWidget::vipcConnected(VisionIpcClient *vipc_client) { @@ -253,18 +255,23 @@ void CameraViewWidget::vipcConnected(VisionIpcClient *vipc_client) { frames.clear(); stream_width = vipc_client->buffers[0].width; stream_height = vipc_client->buffers[0].height; + stream_stride = vipc_client->buffers[0].stride; + + glBindTexture(GL_TEXTURE_2D, textures[0]); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); + glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); + glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); + glTexImage2D(GL_TEXTURE_2D, 0, GL_R8, stream_width, stream_height, 0, GL_RED, GL_UNSIGNED_BYTE, nullptr); + assert(glGetError() == GL_NO_ERROR); - for (int i = 0; i < 3; ++i) { - glBindTexture(GL_TEXTURE_2D, textures[i]); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); - glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); - glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); - int width = i == 0 ? stream_width : stream_width / 2; - int height = i == 0 ? stream_height : stream_height / 2; - glTexImage2D(GL_TEXTURE_2D, 0, GL_R8, width, height, 0, GL_RED, GL_UNSIGNED_BYTE, nullptr); - assert(glGetError() == GL_NO_ERROR); - } + glBindTexture(GL_TEXTURE_2D, textures[1]); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); + glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); + glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); + 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); updateFrameMat(width(), height()); } diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h index 65d5edc221..953cbed00b 100644 --- a/selfdrive/ui/qt/widgets/cameraview.h +++ b/selfdrive/ui/qt/widgets/cameraview.h @@ -49,6 +49,7 @@ protected: std::string stream_name; int stream_width = 0; int stream_height = 0; + int stream_stride = 0; std::atomic stream_type; QThread *vipc_thread = nullptr; diff --git a/selfdrive/ui/replay/framereader.cc b/selfdrive/ui/replay/framereader.cc index ca839e6819..8453407ce7 100644 --- a/selfdrive/ui/replay/framereader.cc +++ b/selfdrive/ui/replay/framereader.cc @@ -229,18 +229,21 @@ AVFrame *FrameReader::decodeFrame(AVPacket *pkt) { bool FrameReader::copyBuffers(AVFrame *f, uint8_t *yuv) { if (hw_pix_fmt == HW_PIX_FMT) { uint8_t *y = yuv ? yuv : nv12toyuv_buffer.data(); - uint8_t *u = y + width * height; - uint8_t *v = u + (width / 2) * (height / 2); - libyuv::NV12ToI420(f->data[0], f->linesize[0], f->data[1], f->linesize[1], - y, width, u, width / 2, v, width / 2, width, height); + uint8_t *uv = y + width * height; + for (int i = 0; i < height/2; i++) { + memcpy(y + (i*2 + 0)*width, f->data[0] + (i*2 + 0)*f->linesize[0], width); + memcpy(y + (i*2 + 1)*width, f->data[0] + (i*2 + 1)*f->linesize[0], width); + memcpy(uv + i*width, f->data[1] + i*f->linesize[1], width); + } } else { - uint8_t *u = yuv + width * height; - uint8_t *v = u + (width / 2) * (height / 2); - libyuv::I420Copy(f->data[0], f->linesize[0], - f->data[1], f->linesize[1], - f->data[2], f->linesize[2], - yuv, width, u, width / 2, v, width / 2, - width, height); + uint8_t *y = yuv ? yuv : nv12toyuv_buffer.data(); + uint8_t *uv = y + width * height; + libyuv::I420ToNV12(f->data[0], f->linesize[0], + f->data[1], f->linesize[1], + f->data[2], f->linesize[2], + y, width, + uv, width / 2, + width, height); } return true; } diff --git a/tools/lib/framereader.py b/tools/lib/framereader.py index e333683b59..d9920ab128 100644 --- a/tools/lib/framereader.py +++ b/tools/lib/framereader.py @@ -185,20 +185,28 @@ def read_file_check_size(f, sz, cookie): return buff -def rgb24toyuv420(rgb): +def rgb24toyuv(rgb): yuv_from_rgb = np.array([[ 0.299 , 0.587 , 0.114 ], [-0.14714119, -0.28886916, 0.43601035 ], [ 0.61497538, -0.51496512, -0.10001026 ]]) img = np.dot(rgb.reshape(-1, 3), yuv_from_rgb.T).reshape(rgb.shape) - y_len = img.shape[0] * img.shape[1] - uv_len = y_len // 4 + ys = img[:, :, 0] us = (img[::2, ::2, 1] + img[1::2, ::2, 1] + img[::2, 1::2, 1] + img[1::2, 1::2, 1]) / 4 + 128 vs = (img[::2, ::2, 2] + img[1::2, ::2, 2] + img[::2, 1::2, 2] + img[1::2, 1::2, 2]) / 4 + 128 - yuv420 = np.empty(y_len + 2 * uv_len, dtype=img.dtype) + return ys, us, vs + + +def rgb24toyuv420(rgb): + ys, us, vs = rgb24toyuv(rgb) + + y_len = rgb.shape[0] * rgb.shape[1] + uv_len = y_len // 4 + + yuv420 = np.empty(y_len + 2 * uv_len, dtype=rgb.dtype) yuv420[:y_len] = ys.reshape(-1) yuv420[y_len:y_len + uv_len] = us.reshape(-1) yuv420[y_len + uv_len:y_len + 2 * uv_len] = vs.reshape(-1) @@ -206,6 +214,20 @@ def rgb24toyuv420(rgb): return yuv420.clip(0, 255).astype('uint8') +def rgb24tonv12(rgb): + ys, us, vs = rgb24toyuv(rgb) + + y_len = rgb.shape[0] * rgb.shape[1] + uv_len = y_len // 4 + + nv12 = np.empty(y_len + 2 * uv_len, dtype=rgb.dtype) + nv12[:y_len] = ys.reshape(-1) + nv12[y_len::2] = us.reshape(-1) + nv12[y_len+1::2] = vs.reshape(-1) + + return nv12.clip(0, 255).astype('uint8') + + def decompress_video_data(rawdat, vid_fmt, w, h, pix_fmt): # using a tempfile is much faster than proc.communicate for some reason @@ -237,6 +259,8 @@ def decompress_video_data(rawdat, vid_fmt, w, h, pix_fmt): if pix_fmt == "rgb24": ret = np.frombuffer(dat, dtype=np.uint8).reshape(-1, h, w, 3) + elif pix_fmt == "nv12": + ret = np.frombuffer(dat, dtype=np.uint8).reshape(-1, (h*w*3//2)) elif pix_fmt == "yuv420p": ret = np.frombuffer(dat, dtype=np.uint8).reshape(-1, (h*w*3//2)) elif pix_fmt == "yuv444p": @@ -304,7 +328,7 @@ class RawFrameReader(BaseFrameReader): assert self.frame_count is not None assert num+count <= self.frame_count - if pix_fmt not in ("yuv420p", "rgb24"): + if pix_fmt not in ("nv12", "yuv420p", "rgb24"): raise ValueError(f"Unsupported pixel format {pix_fmt!r}") app = [] @@ -313,6 +337,8 @@ class RawFrameReader(BaseFrameReader): rgb_dat = self.load_and_debayer(dat) if pix_fmt == "rgb24": app.append(rgb_dat) + elif pix_fmt == "nv12": + app.append(rgb24tonv12(rgb_dat)) elif pix_fmt == "yuv420p": app.append(rgb24toyuv420(rgb_dat)) else: @@ -329,7 +355,7 @@ class VideoStreamDecompressor: self.h = h self.pix_fmt = pix_fmt - if pix_fmt == "yuv420p": + if pix_fmt in ("nv12", "yuv420p"): self.out_size = w*h*3//2 # yuv420p elif pix_fmt in ("rgb24", "yuv444p"): self.out_size = w*h*3 @@ -387,6 +413,8 @@ class VideoStreamDecompressor: ret = np.frombuffer(dat, dtype=np.uint8).reshape((self.h, self.w, 3)) elif self.pix_fmt == "yuv420p": ret = np.frombuffer(dat, dtype=np.uint8) + elif self.pix_fmt == "nv12": + ret = np.frombuffer(dat, dtype=np.uint8) elif self.pix_fmt == "yuv444p": ret = np.frombuffer(dat, dtype=np.uint8).reshape((3, self.h, self.w)) else: @@ -549,7 +577,7 @@ class GOPFrameReader(BaseFrameReader): if num + count > self.frame_count: raise ValueError(f"{num + count} > {self.frame_count}") - if pix_fmt not in ("yuv420p", "rgb24", "yuv444p"): + if pix_fmt not in ("nv12", "yuv420p", "rgb24", "yuv444p"): raise ValueError(f"Unsupported pixel format {pix_fmt!r}") ret = [self._get_one(num + i, pix_fmt) for i in range(count)] From eed6803ccc55ecbfab4bd52a7ea62c9942243aea Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 1 Jun 2022 12:03:34 -0700 Subject: [PATCH 19/29] Button-enable cars: common functions for button events (#24678) * function for creating button events * fix * one line * alphabetical alphabetical * only needed once * common function for enable events * add to list * extend * fixes * add type hints * flip around --- selfdrive/car/__init__.py | 31 +++++++++++++++++++- selfdrive/car/gm/interface.py | 42 ++++++++------------------ selfdrive/car/honda/interface.py | 47 +++++------------------------- selfdrive/car/hyundai/interface.py | 39 +++++-------------------- 4 files changed, 56 insertions(+), 103 deletions(-) diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index 7bb91c40f7..806a060f97 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -1,11 +1,40 @@ # functions common among cars +import capnp + from cereal import car from common.numpy_fast import clip -from typing import Dict +from typing import Dict, List # kg of standard extra cargo to count for drive, gas, etc... STD_CARGO_KG = 136. +ButtonType = car.CarState.ButtonEvent.Type +EventName = car.CarEvent.EventName + + +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: + be = car.CarState.ButtonEvent(pressed=True) + but = cur_but + else: + be = car.CarState.ButtonEvent(pressed=False) + but = prev_but + be.type = buttons_dict.get(but, ButtonType.unknown) + return be + + +def create_button_enable_events(buttonEvents: capnp.lib.capnp._DynamicListBuilder) -> List[int]: + events = [] + for b in buttonEvents: + # do enable on both accel and decel buttons + if b.type in (ButtonType.accelCruise, ButtonType.decelCruise) and not b.pressed: + events.append(EventName.buttonEnable) + # do disable on button down + if b.type == ButtonType.cancel and b.pressed: + events.append(EventName.buttonCancel) + return events + def gen_empty_fingerprint(): return {i: {} for i in range(0, 4)} diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index d00708c336..b4265fc0bd 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -3,12 +3,15 @@ from cereal import car from math import fabs from common.conversions import Conversions as CV -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car import STD_CARGO_KG, create_button_enable_events, create_button_event, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config from selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams from selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName +BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise, + CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel} + class CarInterface(CarInterfaceBase): @staticmethod @@ -159,29 +162,14 @@ class CarInterface(CarInterfaceBase): ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False - buttonEvents = [] - if self.CS.cruise_buttons != self.CS.prev_cruise_buttons and self.CS.prev_cruise_buttons != CruiseButtons.INIT: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.unknown - if self.CS.cruise_buttons != CruiseButtons.UNPRESS: - be.pressed = True - but = self.CS.cruise_buttons - else: - be.pressed = False - but = self.CS.prev_cruise_buttons - if but == CruiseButtons.RES_ACCEL: - if not (ret.cruiseState.enabled and ret.standstill): - be.type = ButtonType.accelCruise # Suppress resume button if we're resuming from stop so we don't adjust speed. - elif but == CruiseButtons.DECEL_SET: - be.type = ButtonType.decelCruise - elif but == CruiseButtons.CANCEL: - be.type = ButtonType.cancel - elif but == CruiseButtons.MAIN: - be.type = ButtonType.altButton3 - buttonEvents.append(be) - - ret.buttonEvents = buttonEvents + be = create_button_event(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT, CruiseButtons.UNPRESS) + + # Suppress resume button if we're resuming from stop so we don't adjust speed. + if be.type == ButtonType.accelCruise and (ret.cruiseState.enabled and ret.standstill): + be.type = ButtonType.unknown + + ret.buttonEvents = [be] events = self.create_common_events(ret, pcm_enable=False) @@ -193,13 +181,7 @@ class CarInterface(CarInterfaceBase): events.add(car.CarEvent.EventName.belowSteerSpeed) # handle button presses - for b in ret.buttonEvents: - # do enable on both accel and decel buttons - if b.type in (ButtonType.accelCruise, ButtonType.decelCruise) and not b.pressed: - events.add(EventName.buttonEnable) - # do disable on button down - if b.type == ButtonType.cancel and b.pressed: - events.add(EventName.buttonCancel) + events.events.extend(create_button_enable_events(ret.buttonEvents)) ret.events = events.to_msg() diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 265175d940..5b97b3389f 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -4,7 +4,7 @@ from panda import Panda from common.conversions import Conversions as CV from common.numpy_fast import interp from selfdrive.car.honda.values import CarControllerParams, CruiseButtons, HondaFlags, CAR, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL -from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car import STD_CARGO_KG, CivicParams, create_button_enable_events, create_button_event, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.disable_ecu import disable_ecu @@ -12,6 +12,8 @@ from selfdrive.car.disable_ecu import disable_ecu ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName TransmissionType = car.CarParams.TransmissionType +BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise, + CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel} class CarInterface(CarInterfaceBase): @@ -334,37 +336,11 @@ class CarInterface(CarInterfaceBase): buttonEvents = [] if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.unknown - if self.CS.cruise_buttons != 0: - be.pressed = True - but = self.CS.cruise_buttons - else: - be.pressed = False - but = self.CS.prev_cruise_buttons - if but == CruiseButtons.RES_ACCEL: - be.type = ButtonType.accelCruise - elif but == CruiseButtons.DECEL_SET: - be.type = ButtonType.decelCruise - elif but == CruiseButtons.CANCEL: - be.type = ButtonType.cancel - elif but == CruiseButtons.MAIN: - be.type = ButtonType.altButton3 - buttonEvents.append(be) + buttonEvents.append(create_button_event(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT)) if self.CS.cruise_setting != self.CS.prev_cruise_setting: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.unknown - if self.CS.cruise_setting != 0: - be.pressed = True - but = self.CS.cruise_setting - else: - be.pressed = False - but = self.CS.prev_cruise_setting - if but == 1: - be.type = ButtonType.altButton1 - # TODO: more buttons? - buttonEvents.append(be) + buttonEvents.append(create_button_event(self.CS.cruise_setting, self.CS.prev_cruise_setting, {1: ButtonType.altButton1})) + ret.buttonEvents = buttonEvents # events @@ -391,16 +367,7 @@ class CarInterface(CarInterfaceBase): events.add(EventName.manualRestart) # handle button presses - for b in ret.buttonEvents: - - # do enable on both accel and decel buttons - if not self.CP.pcmCruise: - if b.type in (ButtonType.accelCruise, ButtonType.decelCruise) and not b.pressed: - events.add(EventName.buttonEnable) - - # do disable on button down - if b.type == ButtonType.cancel and b.pressed: - events.add(EventName.buttonCancel) + events.events.extend(create_button_enable_events(ret.buttonEvents)) ret.events = events.to_msg() diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index b4dc26c87a..7299e29ba8 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -4,7 +4,7 @@ from panda import Panda from common.conversions import Conversions as CV from selfdrive.car.hyundai.values import CAR, DBC, HDA2_CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, Buttons, CarControllerParams from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car import STD_CARGO_KG, create_button_enable_events, create_button_event, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.disable_ecu import disable_ecu from selfdrive.controls.lib.latcontrol_torque import set_torque_tune @@ -12,6 +12,8 @@ from selfdrive.controls.lib.latcontrol_torque import set_torque_tune ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName ENABLE_BUTTONS = (Buttons.RES_ACCEL, Buttons.SET_DECEL, Buttons.CANCEL) +BUTTONS_DICT = {Buttons.RES_ACCEL: ButtonType.accelCruise, Buttons.SET_DECEL: ButtonType.decelCruise, + Buttons.GAP_DIST: ButtonType.gapAdjustCruise, Buttons.CANCEL: ButtonType.cancel} class CarInterface(CarInterfaceBase): @@ -338,37 +340,10 @@ class CarInterface(CarInterfaceBase): if self.CS.brake_error: events.add(EventName.brakeUnavailable) - if self.CS.CP.openpilotLongitudinalControl: - buttonEvents = [] - - if self.CS.cruise_buttons[-1] != self.CS.prev_cruise_buttons: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.unknown - if self.CS.cruise_buttons[-1] != 0: - be.pressed = True - but = self.CS.cruise_buttons[-1] - else: - be.pressed = False - but = self.CS.prev_cruise_buttons - if but == Buttons.RES_ACCEL: - be.type = ButtonType.accelCruise - elif but == Buttons.SET_DECEL: - be.type = ButtonType.decelCruise - elif but == Buttons.GAP_DIST: - be.type = ButtonType.gapAdjustCruise - elif but == Buttons.CANCEL: - be.type = ButtonType.cancel - buttonEvents.append(be) - - ret.buttonEvents = buttonEvents - - for b in ret.buttonEvents: - # do enable on both accel and decel buttons - if b.type in (ButtonType.accelCruise, ButtonType.decelCruise) and not b.pressed: - events.add(EventName.buttonEnable) - # do disable on button down - if b.type == ButtonType.cancel and b.pressed: - events.add(EventName.buttonCancel) + if self.CS.CP.openpilotLongitudinalControl and self.CS.cruise_buttons[-1] != self.CS.prev_cruise_buttons: + ret.buttonEvents = [create_button_event(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT)] + + events.events.extend(create_button_enable_events(ret.buttonEvents)) # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s) if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.: From 7d2ba9b0a43d6c7cfe463968b9fc0c2b6557b00b Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Wed, 1 Jun 2022 13:29:22 -0700 Subject: [PATCH 20/29] Add UDS VIN query (#24311) * Switch to UDS VIN query * try both * fall back to obd --- selfdrive/car/vin.py | 27 +++++++++++++++++---------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/selfdrive/car/vin.py b/selfdrive/car/vin.py index 648f416511..4159af09c0 100755 --- a/selfdrive/car/vin.py +++ b/selfdrive/car/vin.py @@ -1,25 +1,32 @@ #!/usr/bin/env python3 +import struct import traceback import cereal.messaging as messaging +import panda.python.uds as uds from panda.python.uds import FUNCTIONAL_ADDRS from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery from selfdrive.swaglog import cloudlog -VIN_REQUEST = b'\x09\x02' -VIN_RESPONSE = b'\x49\x02\x01' +OBD_VIN_REQUEST = b'\x09\x02' +OBD_VIN_RESPONSE = b'\x49\x02\x01' + +UDS_VIN_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + struct.pack("!H", uds.DATA_IDENTIFIER_TYPE.VIN) +UDS_VIN_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + struct.pack("!H", uds.DATA_IDENTIFIER_TYPE.VIN) + VIN_UNKNOWN = "0" * 17 def get_vin(logcan, sendcan, bus, timeout=0.1, retry=5, debug=False): - for i in range(retry): - try: - query = IsoTpParallelQuery(sendcan, logcan, bus, FUNCTIONAL_ADDRS, [VIN_REQUEST], [VIN_RESPONSE], functional_addr=True, debug=debug) - for addr, vin in query.get_data(timeout).items(): - return addr[0], vin.decode() - print(f"vin query retry ({i+1}) ...") - except Exception: - cloudlog.warning(f"VIN query exception: {traceback.format_exc()}") + for request, response in ((UDS_VIN_REQUEST, UDS_VIN_RESPONSE), (OBD_VIN_REQUEST, OBD_VIN_RESPONSE)): + for i in range(retry): + try: + query = IsoTpParallelQuery(sendcan, logcan, bus, FUNCTIONAL_ADDRS, [request, ], [response, ], functional_addr=True, debug=debug) + for addr, vin in query.get_data(timeout).items(): + return addr[0], vin.decode() + print(f"vin query retry ({i+1}) ...") + except Exception: + cloudlog.warning(f"VIN query exception: {traceback.format_exc()}") return 0, VIN_UNKNOWN From b1177a325a1c7cf604c174b60b5ea2abaab86537 Mon Sep 17 00:00:00 2001 From: Jason Wen <47793918+sunnyhaibin@users.noreply.github.com> Date: Wed, 1 Jun 2022 16:54:15 -0400 Subject: [PATCH 21/29] Hyundai: Add missing engine and transmission FW for 2022 Sonata (#24693) * Hyundai: Add FW for 2022 Sonata * fix duplicate Co-authored-by: Shane Smiskol --- selfdrive/car/hyundai/values.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 76d9aa4f93..1cd2acabeb 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -377,7 +377,7 @@ FW_VERSIONS = { b'\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100', b'\xf1\x00DN ESC \x07 104\x19\x08\x01 58910-L0100', b'\xf1\x00DN ESC \x08 103\x19\x06\x01 58910-L1300', - b'\xf1\x8758910-L0100\xf1\x00DN ESC \a 106 \a\x01 58910-L0100', + b'\xf1\x8758910-L0100\xf1\x00DN ESC \x07 106 \x07\x01 58910-L0100', b'\xf1\x8758910-L0100\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100', b'\xf1\x8758910-L0100\xf1\x00DN ESC \x06 106 \x07\x01 58910-L0100', b'\xf1\x8758910-L0100\xf1\x00DN ESC \x07 104\x19\x08\x01 58910-L0100', @@ -398,6 +398,7 @@ FW_VERSIONS = { b'HM6M1_0a0_F00', b'HM6M1_0a0_G20', b'HM6M2_0a0_BD0', + b'\xf1\x8739110-2S278\xf1\x82DNDVD5GMCCXXXL5B', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00DN8 MDPS C 1,00 1,01 56310L0010\x00 4DNAC101', # modified firmware @@ -479,6 +480,7 @@ FW_VERSIONS = { b'\xf1\x87SAMFBA7978674GJ2gw\x87xgw\x97ywwwwvUGeUUeU\x87O\xfb\xff\x98w\x8f\xfffF\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', b'\xf1\x87SAMFBA9283024GJ2wwwwEUuWwwgwwwwwwwww\x87/\xfb\xff\x98w\x8f\xff<\xd3\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', b'\xf1\x87SAMFBA9708354GJ2wwwwVf\x86h\x88wx\x87xww\x87\x88\x88\x88\x88w/\xfa\xff\x97w\x8f\xff\x86\xa0\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', + b'\xf1\x87SANDB45316691GC6\x99\x99\x99\x99\x88\x88\xa8\x8avfwfwwww\x87wxwT\x9f\xfd\xff\x88wo\xff\x1c\xfa\xf1\x89HT6WAD10A1\xf1\x82SDN8G25NB3\x00\x00\x00\x00\x00\x00', ], }, CAR.SONATA_LF: { From c677c6b164fc8e642ca75809213eca0678cf86ac Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Wed, 1 Jun 2022 14:42:25 -0700 Subject: [PATCH 22/29] remove unused visionimg --- common/SConscript | 1 - common/visionimg.cc | 14 -------------- common/visionimg.h | 16 ---------------- release/files_common | 4 ---- selfdrive/camerad/cameras/camera_common.h | 1 - selfdrive/ui/SConscript | 4 ++-- 6 files changed, 2 insertions(+), 38 deletions(-) delete mode 100644 common/visionimg.cc delete mode 100644 common/visionimg.h diff --git a/common/SConscript b/common/SConscript index ebc0ec79e9..8aee6f42a7 100644 --- a/common/SConscript +++ b/common/SConscript @@ -19,7 +19,6 @@ _common = fxn('common', common_libs, LIBS="json11") files = [ 'clutil.cc', - 'visionimg.cc', ] _gpucommon = fxn('gpucommon', files) diff --git a/common/visionimg.cc b/common/visionimg.cc deleted file mode 100644 index 1009767e0d..0000000000 --- a/common/visionimg.cc +++ /dev/null @@ -1,14 +0,0 @@ -#include "common/visionimg.h" - -#include - -EGLImageTexture::EGLImageTexture(const VisionBuf *buf) { - glGenTextures(1, &frame_tex); - glBindTexture(GL_TEXTURE_2D, frame_tex); - glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, buf->width, buf->height, 0, GL_RGB, GL_UNSIGNED_BYTE, nullptr); - glGenerateMipmap(GL_TEXTURE_2D); -} - -EGLImageTexture::~EGLImageTexture() { - glDeleteTextures(1, &frame_tex); -} diff --git a/common/visionimg.h b/common/visionimg.h deleted file mode 100644 index 0cb41a32b8..0000000000 --- a/common/visionimg.h +++ /dev/null @@ -1,16 +0,0 @@ -#pragma once - -#include "cereal/visionipc/visionbuf.h" - -#ifdef __APPLE__ -#include -#else -#include -#endif - -class EGLImageTexture { - public: - EGLImageTexture(const VisionBuf *buf); - ~EGLImageTexture(); - GLuint frame_tex = 0; -}; diff --git a/release/files_common b/release/files_common index 982194d774..974ea27711 100644 --- a/release/files_common +++ b/release/files_common @@ -146,15 +146,11 @@ common/modeldata.h common/mat.h common/timing.h -common/visionimg.cc -common/visionimg.h - common/gpio.cc common/gpio.h common/i2c.cc common/i2c.h - selfdrive/controls/__init__.py selfdrive/controls/controlsd.py selfdrive/controls/plannerd.py diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h index 85c55f4b03..74d6a6eb3f 100644 --- a/selfdrive/camerad/cameras/camera_common.h +++ b/selfdrive/camerad/cameras/camera_common.h @@ -13,7 +13,6 @@ #include "common/mat.h" #include "common/queue.h" #include "common/swaglog.h" -#include "common/visionimg.h" #include "selfdrive/hardware/hw.h" #define CAMERA_ID_IMX298 0 diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 6b9db34b4d..47c4ac2a51 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -1,8 +1,8 @@ import os -Import('qt_env', 'arch', 'common', 'messaging', 'gpucommon', 'visionipc', +Import('qt_env', 'arch', 'common', 'messaging', 'visionipc', 'cereal', 'transformations') -base_libs = [gpucommon, common, messaging, cereal, visionipc, transformations, 'zmq', +base_libs = [common, messaging, cereal, visionipc, transformations, 'zmq', 'capnp', 'kj', 'm', 'OpenCL', 'ssl', 'crypto', 'pthread'] + qt_env["LIBS"] maps = arch in ['larch64', 'x86_64'] From 3fb22c0289d1c108aa0a5eb2064e7d87cc997bb9 Mon Sep 17 00:00:00 2001 From: ClockeNessMnstr Date: Thu, 2 Jun 2022 00:46:45 -0400 Subject: [PATCH 23/29] process replay: only download used logs (#24661) * only download wanted logs * cleanup * cache as an option * cleanup * Readme * Revert "cache as an option" This reverts commit 060ed4ade548696505e3185fc7d20f1897072ded. * cleanup Co-authored-by: Adeeb Shihadeh --- selfdrive/test/process_replay/README.md | 1 + .../test/process_replay/test_processes.py | 26 +++++++++++-------- 2 files changed, 16 insertions(+), 11 deletions(-) diff --git a/selfdrive/test/process_replay/README.md b/selfdrive/test/process_replay/README.md index e760e4e135..9fa4ca644a 100644 --- a/selfdrive/test/process_replay/README.md +++ b/selfdrive/test/process_replay/README.md @@ -5,6 +5,7 @@ Process replay is a regression test designed to identify any changes in the outp If the test fails, make sure that you didn't unintentionally change anything. If there are intentional changes, the reference logs will be updated. Use `test_processes.py` to run the test locally. +Use `FILEREADER_CACHE='1' test_processes.py` to cache log files. Currently the following processes are tested: diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index afab6cc765..694317d52f 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -131,12 +131,13 @@ def format_diff(results, ref_commit): if __name__ == "__main__": - parser = argparse.ArgumentParser(description="Regression test to identify changes in a process's output") + all_cars = {car for car, _ in segments} + all_procs = {cfg.proc_name for cfg in CONFIGS} - # whitelist has precedence over blacklist in case both are defined - parser.add_argument("--whitelist-procs", type=str, nargs="*", default=[], + parser = argparse.ArgumentParser(description="Regression test to identify changes in a process's output") + parser.add_argument("--whitelist-procs", type=str, nargs="*", default=all_procs, help="Whitelist given processes from the test (e.g. controlsd)") - parser.add_argument("--whitelist-cars", type=str, nargs="*", default=[], + parser.add_argument("--whitelist-cars", type=str, nargs="*", default=all_cars, help="Whitelist given cars from the test (e.g. HONDA)") parser.add_argument("--blacklist-procs", type=str, nargs="*", default=[], help="Blacklist given processes from the test (e.g. controlsd)") @@ -153,6 +154,10 @@ if __name__ == "__main__": parser.add_argument("-j", "--jobs", type=int, default=1) args = parser.parse_args() + tested_procs = set(args.whitelist_procs) - set(args.blacklist_procs) + tested_cars = set(args.whitelist_cars) - set(args.blacklist_cars) + tested_cars = {c.upper() for c in tested_cars} + full_test = all(len(x) == 0 for x in (args.whitelist_procs, args.whitelist_cars, args.blacklist_procs, args.blacklist_cars, args.ignore_fields, args.ignore_msgs)) upload = args.update_refs or args.upload_only os.makedirs(os.path.dirname(FAKEDATA), exist_ok=True) @@ -180,20 +185,19 @@ if __name__ == "__main__": with concurrent.futures.ProcessPoolExecutor(max_workers=args.jobs) as pool: if not args.upload_only: - lreaders: Any = {} - p1 = pool.map(get_logreader, [seg for car, seg in segments]) - for (segment, lr) in tqdm(p1, desc="Getting Logs", total=len(segments)): + download_segments = [seg for car, seg in segments if car in tested_cars] + lreaders: Dict[str, LogReader] = {} + p1 = pool.map(get_logreader, download_segments) + for segment, lr in tqdm(p1, desc="Getting Logs", total=len(download_segments)): lreaders[segment] = lr pool_args: Any = [] for car_brand, segment in segments: - if (len(args.whitelist_cars) and car_brand.upper() not in args.whitelist_cars) or \ - (not len(args.whitelist_cars) and car_brand.upper() in args.blacklist_cars): + if car_brand not in tested_cars: continue for cfg in CONFIGS: - if (len(args.whitelist_procs) and cfg.proc_name not in args.whitelist_procs) or \ - (not len(args.whitelist_procs) and cfg.proc_name in args.blacklist_procs): + if cfg.proc_name not in tested_procs: continue cur_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}_{cur_commit}.bz2") From 86f73a507e2dba56bc8c3a1e753818147fb0eb0f Mon Sep 17 00:00:00 2001 From: Lukas Petersson Date: Thu, 2 Jun 2022 07:02:42 +0200 Subject: [PATCH 24/29] process replay: logreader as bytes (#24610) * willem's changes * classmethod for bytes * submodules * submodules * Update tools/lib/logreader.py Co-authored-by: Adeeb Shihadeh * add back files * little cleanup Co-authored-by: Adeeb Shihadeh --- .../test/process_replay/test_processes.py | 20 +++++++------ tools/lib/logreader.py | 29 +++++++++++-------- 2 files changed, 28 insertions(+), 21 deletions(-) diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 694317d52f..1d5969b1ec 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -12,6 +12,7 @@ from selfdrive.test.openpilotci import get_url, upload_file from selfdrive.test.process_replay.compare_logs import compare_logs, save_log from selfdrive.test.process_replay.process_replay import CONFIGS, PROC_REPLAY_DIR, FAKEDATA, check_enabled, replay_process from selfdrive.version import get_commit +from tools.lib.filereader import FileReader from tools.lib.logreader import LogReader original_segments = [ @@ -57,7 +58,8 @@ REF_COMMIT_FN = os.path.join(PROC_REPLAY_DIR, "ref_commit") def run_test_process(data): - segment, cfg, args, cur_log_fn, ref_log_path, lr = data + segment, cfg, args, cur_log_fn, ref_log_path, lr_dat = data + lr = LogReader.from_bytes(lr_dat) res = None if not args.upload_only: res, log_msgs = test_process(cfg, lr, ref_log_path, args.ignore_fields, args.ignore_msgs) @@ -72,10 +74,10 @@ def run_test_process(data): return (segment, cfg.proc_name, res) -def get_logreader(segment): +def get_log_data(segment): r, n = segment.rsplit("--", 1) - lr = LogReader(get_url(r, n)) - return (segment, lr) + with FileReader(get_url(r, n)) as f: + return (segment, f.read()) def test_process(cfg, lr, ref_log_path, ignore_fields=None, ignore_msgs=None): @@ -186,10 +188,10 @@ if __name__ == "__main__": with concurrent.futures.ProcessPoolExecutor(max_workers=args.jobs) as pool: if not args.upload_only: download_segments = [seg for car, seg in segments if car in tested_cars] - lreaders: Dict[str, LogReader] = {} - p1 = pool.map(get_logreader, download_segments) + log_data: Dict[str, LogReader] = {} + p1 = pool.map(get_log_data, download_segments) for segment, lr in tqdm(p1, desc="Getting Logs", total=len(download_segments)): - lreaders[segment] = lr + log_data[segment] = lr pool_args: Any = [] for car_brand, segment in segments: @@ -207,8 +209,8 @@ if __name__ == "__main__": ref_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}_{ref_commit}.bz2") ref_log_path = ref_log_fn if os.path.exists(ref_log_fn) else BASE_URL + os.path.basename(ref_log_fn) - lr = None if args.upload_only else lreaders[segment] - pool_args.append((segment, cfg, args, cur_log_fn, ref_log_path, lr)) + dat = None if args.upload_only else log_data[segment] + pool_args.append((segment, cfg, args, cur_log_fn, ref_log_path, dat)) results: Any = defaultdict(dict) p2 = pool.map(run_test_process, pool_args) diff --git a/tools/lib/logreader.py b/tools/lib/logreader.py index b4d3de67fb..46c648ef12 100755 --- a/tools/lib/logreader.py +++ b/tools/lib/logreader.py @@ -74,22 +74,24 @@ class MultiLogIterator: class LogReader: - def __init__(self, fn, canonicalize=True, only_union_types=False, sort_by_time=False): + def __init__(self, fn, canonicalize=True, only_union_types=False, sort_by_time=False, dat=None): self.data_version = None self._only_union_types = only_union_types - _, ext = os.path.splitext(urllib.parse.urlparse(fn).path) - with FileReader(fn) as f: - dat = f.read() + ext = None + if not dat: + _, ext = os.path.splitext(urllib.parse.urlparse(fn).path) + if ext not in ('', '.bz2'): + # old rlogs weren't bz2 compressed + raise Exception(f"unknown extension {ext}") - if ext == "": - # old rlogs weren't bz2 compressed - ents = capnp_log.Event.read_multiple_bytes(dat) - elif ext == ".bz2": + with FileReader(fn) as f: + dat = f.read() + + if ext == ".bz2" or dat.startswith(b'BZh9'): dat = bz2.decompress(dat) - ents = capnp_log.Event.read_multiple_bytes(dat) - else: - raise Exception(f"unknown extension {ext}") + + ents = capnp_log.Event.read_multiple_bytes(dat) _ents = [] try: @@ -101,6 +103,10 @@ class LogReader: self._ents = list(sorted(_ents, key=lambda x: x.logMonoTime) if sort_by_time else _ents) self._ts = [x.logMonoTime for x in self._ents] + @classmethod + def from_bytes(cls, dat): + return cls("", dat=dat) + def __iter__(self): for ent in self._ents: if self._only_union_types: @@ -112,7 +118,6 @@ class LogReader: else: yield ent - def logreader_from_route_or_segment(r, sort_by_time=False): sn = SegmentName(r, allow_route_name=True) route = Route(sn.route_name.canonical_name) From dd10a83708a17b04fb9b217c7fee4234b8ed7dfe Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 2 Jun 2022 01:27:34 -0700 Subject: [PATCH 25/29] Hyundai longitudinal: handle mid-press button change (#24674) * send additional button event for previous if we switch buttons mid-press * add comment * remove * comment --- selfdrive/car/hyundai/interface.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 7299e29ba8..673b304e5a 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -341,8 +341,12 @@ class CarInterface(CarInterfaceBase): events.add(EventName.brakeUnavailable) if self.CS.CP.openpilotLongitudinalControl and self.CS.cruise_buttons[-1] != self.CS.prev_cruise_buttons: - ret.buttonEvents = [create_button_event(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT)] + buttonEvents = [create_button_event(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT)] + # Handle CF_Clu_CruiseSwState changing buttons mid-press + if self.CS.cruise_buttons[-1] != 0 and self.CS.prev_cruise_buttons != 0: + buttonEvents.append(create_button_event(0, self.CS.prev_cruise_buttons, BUTTONS_DICT)) + ret.buttonEvents = buttonEvents events.events.extend(create_button_enable_events(ret.buttonEvents)) # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s) From c8c21baf50865b0db2cc9345901bb98904cfbaaf Mon Sep 17 00:00:00 2001 From: Jason Young <46612682+jyoung8607@users.noreply.github.com> Date: Thu, 2 Jun 2022 04:58:47 -0500 Subject: [PATCH 26/29] thermald: consider pmic in component temp management (#24708) --- selfdrive/thermald/thermald.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 7c88dd5790..76573e37e6 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -244,7 +244,7 @@ def thermald_thread(end_event, hw_queue): current_filter.update(msg.deviceState.batteryCurrent / 1e6) max_comp_temp = temp_filter.update( - max(max(msg.deviceState.cpuTempC), msg.deviceState.memoryTempC, max(msg.deviceState.gpuTempC)) + max(max(msg.deviceState.cpuTempC), msg.deviceState.memoryTempC, max(msg.deviceState.gpuTempC), max(msg.deviceState.pmicTempC)) ) if fan_controller is not None: From dc2172b55e1e848476eceb014d8e3eea82c7eb5a Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Thu, 2 Jun 2022 12:20:56 +0200 Subject: [PATCH 27/29] correctly parse UDS VIN response for Honda Bosch (#24710) --- selfdrive/car/vin.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/selfdrive/car/vin.py b/selfdrive/car/vin.py index 4159af09c0..2c5b623b06 100755 --- a/selfdrive/car/vin.py +++ b/selfdrive/car/vin.py @@ -23,6 +23,11 @@ def get_vin(logcan, sendcan, bus, timeout=0.1, retry=5, debug=False): try: query = IsoTpParallelQuery(sendcan, logcan, bus, FUNCTIONAL_ADDRS, [request, ], [response, ], functional_addr=True, debug=debug) for addr, vin in query.get_data(timeout).items(): + + # Honda Bosch response starts with a length, trim to correct length + if vin.startswith(b'\x11'): + vin = vin[1:18] + return addr[0], vin.decode() print(f"vin query retry ({i+1}) ...") except Exception: From f49a9c9fd2100a4f1401a93d88064ddbbbd935c1 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Thu, 2 Jun 2022 15:20:51 +0200 Subject: [PATCH 28/29] less TICI when not needed (#24698) * less TICI when not needed * fix process replay * move reading voltages into hw abstraction layer * Update selfdrive/hardware/tici/hardware.h Co-authored-by: Adeeb Shihadeh * Update selfdrive/hardware/hw.h Co-authored-by: Adeeb Shihadeh * Update selfdrive/hardware/base.h Co-authored-by: Adeeb Shihadeh * rename init function * Update selfdrive/athena/athenad.py Co-authored-by: Robbe Derks * Update selfdrive/boardd/boardd.cc * Apply suggestions from code review * Update selfdrive/thermald/thermald.py * update ref * fix alert width if all cameras are bad * add ecam to test_loggerd * bump cereal * bump cereal Co-authored-by: Adeeb Shihadeh Co-authored-by: Robbe Derks --- SConstruct | 7 ++- cereal | 2 +- common/modeldata.h | 2 - common/realtime.py | 9 +-- common/swaglog.cc | 6 +- common/tests/test_swaglog.cc | 5 +- common/transformations/camera.py | 12 +--- launch_chffrplus.sh | 6 +- selfdrive/athena/athenad.py | 6 +- selfdrive/boardd/boardd.cc | 21 +++---- selfdrive/boardd/main.cc | 2 +- selfdrive/camerad/cameras/camera_common.cc | 6 -- selfdrive/camerad/main.cc | 2 +- selfdrive/camerad/snapshot/snapshot.py | 22 ++----- selfdrive/controls/controlsd.py | 30 +++++----- selfdrive/controls/lib/events.py | 2 +- selfdrive/controls/lib/lane_planner.py | 10 +--- selfdrive/controls/plannerd.py | 3 +- selfdrive/controls/radard.py | 3 +- selfdrive/hardware/__init__.py | 1 + selfdrive/hardware/base.h | 6 ++ selfdrive/hardware/hw.h | 3 + selfdrive/hardware/tici/hardware.h | 6 ++ selfdrive/loggerd/bootlog.cc | 2 +- selfdrive/loggerd/encoderd.cc | 2 +- selfdrive/loggerd/logger.cc | 7 +-- selfdrive/loggerd/loggerd.cc | 4 +- selfdrive/loggerd/loggerd.h | 12 ++-- selfdrive/loggerd/tests/test_loggerd.py | 24 +++----- selfdrive/manager/build.py | 4 +- selfdrive/manager/process_config.py | 6 +- selfdrive/manager/test/test_manager.py | 4 +- selfdrive/modeld/modeld.cc | 2 +- selfdrive/modeld/models/dmonitoring.cc | 58 ++++++------------- selfdrive/monitoring/driver_monitor.py | 17 +++--- .../test/process_replay/process_replay.py | 7 ++- selfdrive/test/process_replay/ref_commit | 2 +- selfdrive/thermald/thermald.py | 6 +- selfdrive/timezoned.py | 4 +- selfdrive/tombstoned.py | 5 +- selfdrive/ui/qt/offroad/driverview.cc | 6 +- selfdrive/ui/qt/offroad/settings.cc | 2 +- selfdrive/ui/qt/widgets/cameraview.cc | 28 +++------ selfdrive/ui/ui.cc | 2 +- selfdrive/updated.py | 4 +- 45 files changed, 142 insertions(+), 238 deletions(-) diff --git a/SConstruct b/SConstruct index d9d05f7940..e6141f7385 100644 --- a/SConstruct +++ b/SConstruct @@ -1,5 +1,4 @@ import os -import shutil import subprocess import sys import sysconfig @@ -7,6 +6,8 @@ import platform import numpy as np TICI = os.path.isfile('/TICI') +AGNOS = TICI + Decider('MD5-timestamp') AddOption('--test', @@ -56,7 +57,7 @@ real_arch = arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rst if platform.system() == "Darwin": arch = "Darwin" -if arch == "aarch64" and TICI: +if arch == "aarch64" and AGNOS: arch = "larch64" USE_WEBCAM = os.getenv("USE_WEBCAM") is not None @@ -226,7 +227,7 @@ if GetOption('compile_db'): env.CompilationDatabase('compile_commands.json') # Setup cache dir -cache_dir = '/data/scons_cache' if TICI else '/tmp/scons_cache' +cache_dir = '/data/scons_cache' if AGNOS else '/tmp/scons_cache' CacheDir(cache_dir) Clean(["."], cache_dir) diff --git a/cereal b/cereal index bf4960f83c..6ad4ba689c 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit bf4960f83ccdefc7e9d2e405878ae6c8efced83b +Subproject commit 6ad4ba689c557ed241862d10aef4235d3e96331f diff --git a/common/modeldata.h b/common/modeldata.h index 4b776ef945..06d9398031 100644 --- a/common/modeldata.h +++ b/common/modeldata.h @@ -24,8 +24,6 @@ constexpr auto T_IDXS_FLOAT = build_idxs(10.0); constexpr auto X_IDXS = build_idxs(192.0); constexpr auto X_IDXS_FLOAT = build_idxs(192.0); -const int TICI_CAM_WIDTH = 1928; - namespace tici_dm_crop { const int x_offset = -72; const int y_offset = -144; diff --git a/common/realtime.py b/common/realtime.py index 8976832141..03051c6f67 100644 --- a/common/realtime.py +++ b/common/realtime.py @@ -8,19 +8,14 @@ from typing import Optional, List, Union from setproctitle import getproctitle # pylint: disable=no-name-in-module from common.clock import sec_since_boot # pylint: disable=no-name-in-module, import-error -from selfdrive.hardware import PC, TICI +from selfdrive.hardware import PC # time step for each process DT_CTRL = 0.01 # controlsd DT_MDL = 0.05 # model DT_TRML = 0.5 # thermald and manager - -# driver monitoring -if TICI: - DT_DMON = 0.05 -else: - DT_DMON = 0.1 +DT_DMON = 0.05 # driver monitoring class Priority: diff --git a/common/swaglog.cc b/common/swaglog.cc index f0d0f0f508..75223854fe 100644 --- a/common/swaglog.cc +++ b/common/swaglog.cc @@ -50,11 +50,7 @@ class SwaglogState : public LogState { ctx_j["dirty"] = !getenv("CLEAN"); // device type - if (Hardware::TICI()) { - ctx_j["device"] = "tici"; - } else { - ctx_j["device"] = "pc"; - } + ctx_j["device"] = Hardware::get_name(); LogState::initialize(); } }; diff --git a/common/tests/test_swaglog.cc b/common/tests/test_swaglog.cc index a95ae45e03..b54d1d6338 100644 --- a/common/tests/test_swaglog.cc +++ b/common/tests/test_swaglog.cc @@ -56,10 +56,7 @@ void recv_log(int thread_cnt, int thread_msg_cnt) { REQUIRE(ctx["version"].string_value() == COMMA_VERSION); - std::string device = "pc"; - if (Hardware::TICI()) { - device = "tici"; - } + std::string device = Hardware::get_name(); REQUIRE(ctx["device"].string_value() == device); int thread_id = atoi(msg["msg"].string_value().c_str()); diff --git a/common/transformations/camera.py b/common/transformations/camera.py index 7573877a3e..5d100d1047 100644 --- a/common/transformations/camera.py +++ b/common/transformations/camera.py @@ -1,7 +1,6 @@ import numpy as np import common.transformations.orientation as orient -from selfdrive.hardware import TICI ## -- hardcoded hardware params -- eon_f_focal_length = 910.0 @@ -45,14 +44,9 @@ tici_fcam_intrinsics_inv = np.linalg.inv(tici_fcam_intrinsics) tici_ecam_intrinsics_inv = np.linalg.inv(tici_ecam_intrinsics) -if not TICI: - FULL_FRAME_SIZE = eon_f_frame_size - FOCAL = eon_f_focal_length - fcam_intrinsics = eon_fcam_intrinsics -else: - FULL_FRAME_SIZE = tici_f_frame_size - FOCAL = tici_f_focal_length - fcam_intrinsics = tici_fcam_intrinsics +FULL_FRAME_SIZE = tici_f_frame_size +FOCAL = tici_f_focal_length +fcam_intrinsics = tici_fcam_intrinsics W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1] diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 503ebb5d41..a37e27c4fe 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -8,7 +8,7 @@ source "$BASEDIR/launch_env.sh" DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" -function tici_init { +function agnos_init { # wait longer for weston to come up if [ -f "$BASEDIR/prebuilt" ]; then sleep 3 @@ -77,9 +77,7 @@ function launch { export PYTHONPATH="$PWD:$PWD/pyextra" # hardware specific init - if [ -f /TICI ]; then - tici_init - fi + agnos_init # write tmux scrollback to a file tmux capture-pane -pq -S-1000 > /tmp/launch_log diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index c6936d5d40..ba86e02cc6 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -32,7 +32,7 @@ from common.basedir import PERSIST from common.file_helpers import CallbackReader from common.params import Params from common.realtime import sec_since_boot, set_core_affinity -from selfdrive.hardware import HARDWARE, PC, TICI +from selfdrive.hardware import HARDWARE, PC, AGNOS from selfdrive.loggerd.config import ROOT from selfdrive.loggerd.xattr_cache import getxattr, setxattr from selfdrive.statsd import STATS_DIR @@ -413,8 +413,8 @@ def primeActivated(activated): @dispatcher.add_method def setBandwithLimit(upload_speed_kbps, download_speed_kbps): - if not TICI: - return {"success": 0, "error": "only supported on comma three"} + if not AGNOS: + return {"success": 0, "error": "only supported on AGNOS"} try: HARDWARE.set_bandwidth_limit(upload_speed_kbps, download_speed_kbps) diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 2622f7eba1..2745c037b6 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -405,17 +405,12 @@ void send_peripheral_state(PubMaster *pm, Panda *panda) { auto ps = evt.initPeripheralState(); ps.setPandaType(panda->hw_type); - if (Hardware::TICI()) { - double read_time = millis_since_boot(); - ps.setVoltage(std::atoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input").c_str())); - ps.setCurrent(std::atoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input").c_str())); - read_time = millis_since_boot() - read_time; - if (read_time > 50) { - LOGW("reading hwmon took %lfms", read_time); - } - } else { - ps.setVoltage(pandaState.voltage_pkt); - ps.setCurrent(pandaState.current_pkt); + double read_time = millis_since_boot(); + ps.setVoltage(Hardware::get_voltage()); + ps.setCurrent(Hardware::get_current()); + read_time = millis_since_boot() - read_time; + if (read_time > 50) { + LOGW("reading hwmon took %lfms", read_time); } uint16_t fan_speed_rpm = panda->get_fan_speed(); @@ -534,9 +529,7 @@ void peripheral_control_thread(Panda *panda) { int cur_integ_lines = event.getDriverCameraState().getIntegLines(); float cur_gain = event.getDriverCameraState().getGain(); - if (Hardware::TICI()) { - cur_integ_lines = integ_lines_filter.update(cur_integ_lines * cur_gain); - } + cur_integ_lines = integ_lines_filter.update(cur_integ_lines * cur_gain); last_front_frame_t = event.getLogMonoTime(); if (cur_integ_lines <= CUTOFF_IL) { diff --git a/selfdrive/boardd/main.cc b/selfdrive/boardd/main.cc index b151832b72..6f1f83685b 100644 --- a/selfdrive/boardd/main.cc +++ b/selfdrive/boardd/main.cc @@ -12,7 +12,7 @@ int main(int argc, char *argv[]) { int err; err = util::set_realtime_priority(54); assert(err == 0); - err = util::set_core_affinity({Hardware::TICI() ? 4 : 3}); + err = util::set_core_affinity({4}); assert(err == 0); } diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc index 40a3b86cdd..049324f085 100644 --- a/selfdrive/camerad/cameras/camera_common.cc +++ b/selfdrive/camerad/cameras/camera_common.cc @@ -90,12 +90,6 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, rgb_width = ci->frame_width; rgb_height = ci->frame_height; - if (!Hardware::TICI() && ci->bayer) { - // debayering does a 2x downscale - rgb_width = ci->frame_width / 2; - rgb_height = ci->frame_height / 2; - } - yuv_transform = get_model_yuv_transform(ci->bayer); vipc_server->create_buffers(rgb_type, UI_BUF_COUNT, true, rgb_width, rgb_height); diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc index f59e03f8fa..c86543265c 100644 --- a/selfdrive/camerad/main.cc +++ b/selfdrive/camerad/main.cc @@ -7,7 +7,7 @@ #include "selfdrive/hardware/hw.h" int main(int argc, char *argv[]) { - if (Hardware::TICI()) { + if (!Hardware::PC()) { int ret; ret = util::set_realtime_priority(53); assert(ret == 0); diff --git a/selfdrive/camerad/snapshot/snapshot.py b/selfdrive/camerad/snapshot/snapshot.py index bf37ca181e..9f9072c962 100755 --- a/selfdrive/camerad/snapshot/snapshot.py +++ b/selfdrive/camerad/snapshot/snapshot.py @@ -4,13 +4,12 @@ import time import numpy as np from PIL import Image -from typing import List import cereal.messaging as messaging from cereal.visionipc import VisionIpcClient, VisionStreamType from common.params import Params from common.realtime import DT_MDL -from selfdrive.hardware import TICI, PC +from selfdrive.hardware import PC from selfdrive.controls.lib.alertmanager import set_offroad_alert from selfdrive.manager.process_config import managed_processes @@ -52,11 +51,7 @@ def extract_image(buf, w, h, stride, uv_offset): return yuv_to_rgb(y, u, v) -def rois_in_focus(lapres: List[float]) -> float: - return sum(1. / len(lapres) for sharpness in lapres if sharpness >= LM_THRESH) - - -def get_snapshots(frame="roadCameraState", front_frame="driverCameraState", focus_perc_threshold=0.): +def get_snapshots(frame="roadCameraState", front_frame="driverCameraState"): sockets = [s for s in (frame, front_frame) if s is not None] sm = messaging.SubMaster(sockets) vipc_clients = {s: VisionIpcClient("camerad", VISION_STREAMS[s], True) for s in sockets} @@ -68,13 +63,6 @@ def get_snapshots(frame="roadCameraState", front_frame="driverCameraState", focu for client in vipc_clients.values(): client.connect(True) - # wait for focus - start_t = time.monotonic() - while time.monotonic() - start_t < 10: - sm.update(100) - if min(sm.rcv_frame.values()) > 1 and rois_in_focus(sm[frame].sharpnessScore) >= focus_perc_threshold: - break - # grab images rear, front = None, None if frame is not None: @@ -113,11 +101,9 @@ def snapshot(): if not PC: managed_processes['camerad'].start() - frame = "wideRoadCameraState" if TICI else "roadCameraState" + frame = "wideRoadCameraState" front_frame = "driverCameraState" if front_camera_allowed else None - focus_perc_threshold = 0. if TICI else 10 / 12. - - rear, front = get_snapshots(frame, front_frame, focus_perc_threshold) + rear, front = get_snapshots(frame, front_frame) finally: managed_processes['camerad'].stop() params.put_bool("IsTakingSnapshot", False) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index fc0f289e39..edb1538c31 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -27,7 +27,7 @@ from selfdrive.controls.lib.events import Events, ET from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.locationd.calibrationd import Calibration -from selfdrive.hardware import HARDWARE, TICI +from selfdrive.hardware import HARDWARE from selfdrive.manager.process_config import managed_processes SOFT_DISABLE_TIME = 3 # seconds @@ -68,17 +68,14 @@ class Controls: self.pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', 'carControl', 'carEvents', 'carParams']) - self.camera_packets = ["roadCameraState", "driverCameraState"] - if TICI: - self.camera_packets.append("wideRoadCameraState") + self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"] self.can_sock = can_sock if can_sock is None: can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 20 self.can_sock = messaging.sub_sock('can', timeout=can_timeout) - if TICI: - self.log_sock = messaging.sub_sock('androidLog') + self.log_sock = messaging.sub_sock('androidLog') if CI is None: # wait for one pandaState and one CAN packet @@ -358,17 +355,16 @@ class Controls: if planner_fcw or model_fcw: self.events.add(EventName.fcw) - if TICI: - for m in messaging.drain_sock(self.log_sock, wait_for_one=False): - try: - msg = m.androidLog.message - if any(err in msg for err in ("ERROR_CRC", "ERROR_ECC", "ERROR_STREAM_UNDERFLOW", "APPLY FAILED")): - csid = msg.split("CSID:")[-1].split(" ")[0] - evt = CSID_MAP.get(csid, None) - if evt is not None: - self.events.add(evt) - except UnicodeDecodeError: - pass + for m in messaging.drain_sock(self.log_sock, wait_for_one=False): + try: + msg = m.androidLog.message + if any(err in msg for err in ("ERROR_CRC", "ERROR_ECC", "ERROR_STREAM_UNDERFLOW", "APPLY FAILED")): + csid = msg.split("CSID:")[-1].split(" ")[0] + evt = CSID_MAP.get(csid, None) + if evt is not None: + self.events.add(evt) + except UnicodeDecodeError: + pass # TODO: fix simulator if not SIMULATION: diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 8a37f11fdb..7ca05cc744 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -284,7 +284,7 @@ def comm_issue_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaste def camera_malfunction_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: all_cams = ('roadCameraState', 'driverCameraState', 'wideRoadCameraState') - bad_cams = [s for s in all_cams if s in sm.data.keys() and not sm.all_checks([s, ])] + bad_cams = [s.replace('State', '') for s in all_cams if s in sm.data.keys() and not sm.all_checks([s, ])] return NormalPermanentAlert("Camera Malfunction", ', '.join(bad_cams)) diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index aedf61a073..9bbad59084 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -3,20 +3,14 @@ from cereal import log from common.filter_simple import FirstOrderFilter from common.numpy_fast import interp from common.realtime import DT_MDL -from selfdrive.hardware import TICI from selfdrive.swaglog import cloudlog TRAJECTORY_SIZE = 33 # camera offset is meters from center car to camera -# model path is in the frame of the camera. Empirically -# the model knows the difference between TICI and EON -# so a path offset is not needed +# model path is in the frame of the camera PATH_OFFSET = 0.00 -if TICI: - CAMERA_OFFSET = 0.04 -else: - CAMERA_OFFSET = 0.0 +CAMERA_OFFSET = 0.04 class LanePlanner: diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 32438fa799..1fc79decef 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -5,12 +5,11 @@ from common.realtime import Priority, config_realtime_process from selfdrive.swaglog import cloudlog from selfdrive.controls.lib.longitudinal_planner import Planner from selfdrive.controls.lib.lateral_planner import LateralPlanner -from selfdrive.hardware import TICI import cereal.messaging as messaging def plannerd_thread(sm=None, pm=None): - config_realtime_process(5 if TICI else 2, Priority.CTRL_LOW) + config_realtime_process(5, Priority.CTRL_LOW) cloudlog.info("plannerd is waiting for CarParams") params = Params() diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index cae9474faf..0e514193dc 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -11,7 +11,6 @@ from common.realtime import Ratekeeper, Priority, config_realtime_process from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid from selfdrive.controls.lib.radar_helpers import Cluster, Track, RADAR_TO_CAMERA from selfdrive.swaglog import cloudlog -from selfdrive.hardware import TICI class KalmanParams(): @@ -180,7 +179,7 @@ class RadarD(): # fuses camera and radar data for best lead detection def radard_thread(sm=None, pm=None, can_sock=None): - config_realtime_process(5 if TICI else 2, Priority.CTRL_LOW) + config_realtime_process(5, Priority.CTRL_LOW) # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") diff --git a/selfdrive/hardware/__init__.py b/selfdrive/hardware/__init__.py index 03dfce86d7..a1bf2e912f 100644 --- a/selfdrive/hardware/__init__.py +++ b/selfdrive/hardware/__init__.py @@ -6,6 +6,7 @@ from selfdrive.hardware.tici.hardware import Tici from selfdrive.hardware.pc.hardware import Pc TICI = os.path.isfile('/TICI') +AGNOS = TICI PC = not TICI diff --git a/selfdrive/hardware/base.h b/selfdrive/hardware/base.h index 2826d09700..b70948d482 100644 --- a/selfdrive/hardware/base.h +++ b/selfdrive/hardware/base.h @@ -2,6 +2,7 @@ #include #include +#include "cereal/messaging/messaging.h" // no-op base hw class class HardwareNone { @@ -10,6 +11,10 @@ public: static constexpr float MIN_VOLUME = 0.2; static std::string get_os_version() { return ""; } + static std::string get_name() { return ""; }; + static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::UNKNOWN; }; + static int get_voltage() { return 0; }; + static int get_current() { return 0; }; static void reboot() {} static void poweroff() {} @@ -21,4 +26,5 @@ public: static bool PC() { return false; } static bool TICI() { return false; } + static bool AGNOS() { return false; } }; diff --git a/selfdrive/hardware/hw.h b/selfdrive/hardware/hw.h index 5f178ec0f9..364a1c5c33 100644 --- a/selfdrive/hardware/hw.h +++ b/selfdrive/hardware/hw.h @@ -10,8 +10,11 @@ class HardwarePC : public HardwareNone { public: static std::string get_os_version() { return "openpilot for PC"; } + static std::string get_name() { return "pc"; }; + static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::PC; }; static bool PC() { return true; } static bool TICI() { return util::getenv("TICI", 0) == 1; } + static bool AGNOS() { return util::getenv("TICI", 0) == 1; } }; #define Hardware HardwarePC #endif diff --git a/selfdrive/hardware/tici/hardware.h b/selfdrive/hardware/tici/hardware.h index ec53e9ae8f..b11a0a5bb0 100644 --- a/selfdrive/hardware/tici/hardware.h +++ b/selfdrive/hardware/tici/hardware.h @@ -12,9 +12,15 @@ public: static constexpr float MAX_VOLUME = 0.9; static constexpr float MIN_VOLUME = 0.2; static bool TICI() { return true; } + static bool AGNOS() { return true; } static std::string get_os_version() { return "AGNOS " + util::read_file("/VERSION"); }; + static std::string get_name() { return "tici"; }; + static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::TICI; }; + static int get_voltage() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input").c_str()); }; + static int get_current() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input").c_str()); }; + static void reboot() { std::system("sudo reboot"); }; static void poweroff() { std::system("sudo poweroff"); }; diff --git a/selfdrive/loggerd/bootlog.cc b/selfdrive/loggerd/bootlog.cc index eee6b86720..882b749fe9 100644 --- a/selfdrive/loggerd/bootlog.cc +++ b/selfdrive/loggerd/bootlog.cc @@ -8,7 +8,7 @@ static kj::Array build_boot_log() { std::vector bootlog_commands; - if (Hardware::TICI()) { + if (Hardware::AGNOS()) { bootlog_commands.push_back("journalctl"); bootlog_commands.push_back("sudo nvme smart-log --output-format=json /dev/nvme0"); } diff --git a/selfdrive/loggerd/encoderd.cc b/selfdrive/loggerd/encoderd.cc index 5eb70dad33..87cf4a492f 100644 --- a/selfdrive/loggerd/encoderd.cc +++ b/selfdrive/loggerd/encoderd.cc @@ -133,7 +133,7 @@ void encoderd_thread() { } int main() { - if (Hardware::TICI()) { + if (!Hardware::PC()) { int ret; ret = util::set_realtime_priority(52); assert(ret == 0); diff --git a/selfdrive/loggerd/logger.cc b/selfdrive/loggerd/logger.cc index d8a409e93e..5aed47e291 100644 --- a/selfdrive/loggerd/logger.cc +++ b/selfdrive/loggerd/logger.cc @@ -33,12 +33,7 @@ kj::Array logger_build_init_data() { MessageBuilder msg; auto init = msg.initEvent().initInitData(); - if (Hardware::TICI()) { - init.setDeviceType(cereal::InitData::DeviceType::TICI); - } else { - init.setDeviceType(cereal::InitData::DeviceType::PC); - } - + init.setDeviceType(Hardware::get_device_type()); init.setVersion(COMMA_VERSION); std::ifstream cmdline_stream("/proc/cmdline"); diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc index 1a10ab0c07..4086f4991e 100644 --- a/selfdrive/loggerd/loggerd.cc +++ b/selfdrive/loggerd/loggerd.cc @@ -267,7 +267,7 @@ void loggerd_thread() { } int main(int argc, char** argv) { - if (Hardware::TICI()) { + if (!Hardware::PC()) { int ret; ret = util::set_core_affinity({0, 1, 2, 3}); assert(ret == 0); @@ -279,4 +279,4 @@ int main(int argc, char** argv) { loggerd_thread(); return 0; -} \ No newline at end of file +} diff --git a/selfdrive/loggerd/loggerd.h b/selfdrive/loggerd/loggerd.h index 21aa66ea12..3b9b01a0fc 100644 --- a/selfdrive/loggerd/loggerd.h +++ b/selfdrive/loggerd/loggerd.h @@ -33,8 +33,8 @@ #endif constexpr int MAIN_FPS = 20; -const int MAIN_BITRATE = Hardware::TICI() ? 10000000 : 5000000; -const int DCAM_BITRATE = Hardware::TICI() ? MAIN_BITRATE : 2500000; +const int MAIN_BITRATE = 10000000; +const int DCAM_BITRATE = MAIN_BITRATE; #define NO_CAMERA_PATIENCE 500 // fall back to time-based rotation if all cameras are dead @@ -89,8 +89,8 @@ const LogCameraInfo cameras_logged[] = { .bitrate = MAIN_BITRATE, .is_h265 = true, .has_qcamera = false, - .enable = Hardware::TICI(), - .record = Hardware::TICI(), + .enable = true, + .record = true, .frame_width = 1928, .frame_height = 1208, }, @@ -102,6 +102,6 @@ const LogCameraInfo qcam_info = { .is_h265 = false, .enable = true, .record = true, - .frame_width = Hardware::TICI() ? 526 : 480, - .frame_height = Hardware::TICI() ? 330 : 360 // keep pixel count the same? + .frame_width = 526, + .frame_height = 330, }; diff --git a/selfdrive/loggerd/tests/test_loggerd.py b/selfdrive/loggerd/tests/test_loggerd.py index 8eecd834e1..54f7aaaa2b 100755 --- a/selfdrive/loggerd/tests/test_loggerd.py +++ b/selfdrive/loggerd/tests/test_loggerd.py @@ -15,14 +15,12 @@ from cereal.services import service_list from common.basedir import BASEDIR from common.params import Params from common.timeout import Timeout -from selfdrive.hardware import TICI from selfdrive.loggerd.config import ROOT from selfdrive.manager.process_config import managed_processes from selfdrive.version import get_version from tools.lib.logreader import LogReader from cereal.visionipc import VisionIpcServer, VisionStreamType -from common.transformations.camera import eon_f_frame_size, tici_f_frame_size, \ - eon_d_frame_size, tici_d_frame_size, tici_e_frame_size +from common.transformations.camera import tici_f_frame_size, tici_d_frame_size, tici_e_frame_size SentinelType = log.Sentinel.SentinelType @@ -108,20 +106,15 @@ class TestLoggerd(unittest.TestCase): os.environ["LOGGERD_TEST"] = "1" Params().put("RecordFront", "1") - expected_files = {"rlog", "qlog", "qcamera.ts", "fcamera.hevc", "dcamera.hevc"} - streams = [(VisionStreamType.VISION_STREAM_ROAD, (*tici_f_frame_size, 2048*2346, 2048, 2048*1216) if TICI else eon_f_frame_size, "roadCameraState"), - (VisionStreamType.VISION_STREAM_DRIVER, (*tici_d_frame_size, 2048*2346, 2048, 2048*1216) if TICI else eon_d_frame_size, "driverCameraState")] - if TICI: - expected_files.add("ecamera.hevc") - streams.append((VisionStreamType.VISION_STREAM_WIDE_ROAD, (*tici_e_frame_size, 2048*2346, 2048, 2048*1216), "wideRoadCameraState")) + expected_files = {"rlog", "qlog", "qcamera.ts", "fcamera.hevc", "dcamera.hevc", "ecamera.hevc"} + streams = [(VisionStreamType.VISION_STREAM_ROAD, (*tici_f_frame_size, 2048*2346, 2048, 2048*1216), "roadCameraState"), + (VisionStreamType.VISION_STREAM_DRIVER, (*tici_d_frame_size, 2048*2346, 2048, 2048*1216), "driverCameraState"), + (VisionStreamType.VISION_STREAM_WIDE_ROAD, (*tici_e_frame_size, 2048*2346, 2048, 2048*1216), "wideRoadCameraState")] pm = messaging.PubMaster(["roadCameraState", "driverCameraState", "wideRoadCameraState"]) vipc_server = VisionIpcServer("camerad") for stream_type, frame_spec, _ in streams: - if TICI: - vipc_server.create_buffers_with_sizes(stream_type, 40, False, *(frame_spec)) - else: - vipc_server.create_buffers(stream_type, 40, False, *(frame_spec)) + vipc_server.create_buffers_with_sizes(stream_type, 40, False, *(frame_spec)) vipc_server.start_listener() for _ in range(5): @@ -134,10 +127,7 @@ class TestLoggerd(unittest.TestCase): fps = 20.0 for n in range(1, int(num_segs*length*fps)+1): for stream_type, frame_spec, state in streams: - if TICI: - dat = np.empty(frame_spec[2], dtype=np.uint8) - else: - dat = np.empty(int(frame_spec[0]*frame_spec[1]*3/2), dtype=np.uint8) + dat = np.empty(frame_spec[2], dtype=np.uint8) vipc_server.send(stream_type, dat[:].flatten().tobytes(), n, n/fps, n/fps) camera_state = messaging.new_message(state) diff --git a/selfdrive/manager/build.py b/selfdrive/manager/build.py index 806aa58bb7..bcc62a2932 100755 --- a/selfdrive/manager/build.py +++ b/selfdrive/manager/build.py @@ -10,12 +10,12 @@ from pathlib import Path from common.basedir import BASEDIR from common.spinner import Spinner from common.text_window import TextWindow -from selfdrive.hardware import TICI +from selfdrive.hardware import AGNOS from selfdrive.swaglog import cloudlog, add_file_handler from selfdrive.version import is_dirty MAX_CACHE_SIZE = 4e9 if "CI" in os.environ else 2e9 -CACHE_DIR = Path("/data/scons_cache" if TICI else "/tmp/scons_cache") +CACHE_DIR = Path("/data/scons_cache" if AGNOS else "/tmp/scons_cache") TOTAL_SCONS_NODES = 2405 MAX_BUILD_PROGRESS = 100 diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 7f31d837b5..48524aec1d 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -2,7 +2,7 @@ import os from cereal import car from common.params import Params -from selfdrive.hardware import TICI, PC +from selfdrive.hardware import PC from selfdrive.manager.process import PythonProcess, NativeProcess, DaemonProcess WEBCAM = os.getenv("USE_WEBCAM") is not None @@ -30,7 +30,7 @@ procs = [ NativeProcess("proclogd", "selfdrive/proclogd", ["./proclogd"]), NativeProcess("sensord", "selfdrive/sensord", ["./sensord"], enabled=not PC), NativeProcess("ubloxd", "selfdrive/locationd", ["./ubloxd"], enabled=(not PC or WEBCAM)), - NativeProcess("ui", "selfdrive/ui", ["./ui"], offroad=True, watchdog_max_dt=(5 if TICI else None)), + NativeProcess("ui", "selfdrive/ui", ["./ui"], offroad=True, watchdog_max_dt=(5 if not PC else None)), NativeProcess("soundd", "selfdrive/ui/soundd", ["./soundd"], offroad=True), NativeProcess("locationd", "selfdrive/locationd", ["./locationd"]), NativeProcess("boardd", "selfdrive/boardd", ["./boardd"], enabled=False), @@ -45,7 +45,7 @@ procs = [ PythonProcess("plannerd", "selfdrive.controls.plannerd"), PythonProcess("radard", "selfdrive.controls.radard"), PythonProcess("thermald", "selfdrive.thermald.thermald", offroad=True), - PythonProcess("timezoned", "selfdrive.timezoned", enabled=TICI, offroad=True), + PythonProcess("timezoned", "selfdrive.timezoned", enabled=not PC, offroad=True), PythonProcess("tombstoned", "selfdrive.tombstoned", enabled=not PC, offroad=True), PythonProcess("updated", "selfdrive.updated", enabled=not PC, onroad=False, offroad=True), PythonProcess("uploader", "selfdrive.loggerd.uploader", offroad=True), diff --git a/selfdrive/manager/test/test_manager.py b/selfdrive/manager/test/test_manager.py index c1a6d4816b..31949de0b9 100755 --- a/selfdrive/manager/test/test_manager.py +++ b/selfdrive/manager/test/test_manager.py @@ -5,7 +5,7 @@ import time import unittest import selfdrive.manager.manager as manager -from selfdrive.hardware import TICI, HARDWARE +from selfdrive.hardware import AGNOS, HARDWARE from selfdrive.manager.process import DaemonProcess from selfdrive.manager.process_config import managed_processes @@ -50,7 +50,7 @@ class TestManager(unittest.TestCase): self.assertTrue(state.running, f"{p} not running") exit_code = managed_processes[p].stop(retry=False) - if (TICI and p in ['ui',]): + if (AGNOS and p in ['ui',]): # TODO: make Qt UI exit gracefully continue diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index 593abbc9e4..10cc2fe56e 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -163,7 +163,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl } int main(int argc, char **argv) { - if (Hardware::TICI()) { + if (!Hardware::PC()) { int ret; ret = util::set_realtime_priority(54); assert(ret == 0); diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc index 4cc1cd1229..e134ad3a5a 100644 --- a/selfdrive/modeld/models/dmonitoring.cc +++ b/selfdrive/modeld/models/dmonitoring.cc @@ -69,22 +69,13 @@ void crop_nv12_to_yuv(uint8_t *raw, int stride, int uv_offset, uint8_t *y, uint8 } DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, int stride, int uv_offset, float *calib) { - Rect crop_rect; - if (width == TICI_CAM_WIDTH) { - const int cropped_height = tici_dm_crop::width / 1.33; - crop_rect = {width / 2 - tici_dm_crop::width / 2 + tici_dm_crop::x_offset, - height / 2 - cropped_height / 2 + tici_dm_crop::y_offset, - cropped_height / 2, - cropped_height}; - if (!s->is_rhd) { - crop_rect.x += tici_dm_crop::width - crop_rect.w; - } - } else { - const int adapt_width = 372; - crop_rect = {0, 0, adapt_width, height}; - if (!s->is_rhd) { - crop_rect.x += width - crop_rect.w; - } + const int cropped_height = tici_dm_crop::width / 1.33; + Rect crop_rect = {width / 2 - tici_dm_crop::width / 2 + tici_dm_crop::x_offset, + height / 2 - cropped_height / 2 + tici_dm_crop::y_offset, + cropped_height / 2, + cropped_height}; + if (!s->is_rhd) { + crop_rect.x += tici_dm_crop::width - crop_rect.w; } int resized_width = MODEL_WIDTH; @@ -108,31 +99,16 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_ auto [resized_buf, resized_u, resized_v] = get_yuv_buf(s->resized_buf, resized_width, resized_height); uint8_t *resized_y = resized_buf; libyuv::FilterMode mode = libyuv::FilterModeEnum::kFilterBilinear; - if (Hardware::TICI()) { - libyuv::I420Scale(cropped_y, crop_rect.w, - cropped_u, crop_rect.w / 2, - cropped_v, crop_rect.w / 2, - crop_rect.w, crop_rect.h, - resized_y, resized_width, - resized_u, resized_width / 2, - resized_v, resized_width / 2, - resized_width, resized_height, - mode); - } else { - const int source_height = 0.7*resized_height; - const int extra_height = (resized_height - source_height) / 2; - const int extra_width = (resized_width - source_height / 2) / 2; - const int source_width = source_height / 2 + extra_width; - libyuv::I420Scale(cropped_y, crop_rect.w, - cropped_u, crop_rect.w / 2, - cropped_v, crop_rect.w / 2, - crop_rect.w, crop_rect.h, - resized_y + extra_height * resized_width, resized_width, - resized_u + extra_height / 2 * resized_width / 2, resized_width / 2, - resized_v + extra_height / 2 * resized_width / 2, resized_width / 2, - source_width, source_height, - mode); - } + libyuv::I420Scale(cropped_y, crop_rect.w, + cropped_u, crop_rect.w / 2, + cropped_v, crop_rect.w / 2, + crop_rect.w, crop_rect.h, + resized_y, resized_width, + resized_u, resized_width / 2, + resized_v, resized_width / 2, + resized_width, resized_height, + mode); + int yuv_buf_len = (MODEL_WIDTH/2) * (MODEL_HEIGHT/2) * 6; // Y|u|v -> y|y|y|y|u|v float *net_input_buf = get_buffer(s->net_input_buf, yuv_buf_len); diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py index 66b28c39b7..662e0d76ce 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/selfdrive/monitoring/driver_monitor.py @@ -3,7 +3,6 @@ from math import atan2 from cereal import car from common.numpy_fast import interp from common.realtime import DT_DMON -from selfdrive.hardware import TICI from common.filter_simple import FirstOrderFilter from common.stat_live import RunningStatFilter @@ -16,7 +15,7 @@ EventName = car.CarEvent.EventName # ****************************************************************************************** class DRIVER_MONITOR_SETTINGS(): - def __init__(self, TICI=TICI, DT_DMON=DT_DMON): + def __init__(self): self._DT_DMON = DT_DMON # ref (page15-16): https://eur-lex.europa.eu/legal-content/EN/TXT/PDF/?uri=CELEX:42018X1947&rid=2 self._AWARENESS_TIME = 30. # passive wheeltouch total timeout @@ -27,15 +26,15 @@ class DRIVER_MONITOR_SETTINGS(): self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6. self._FACE_THRESHOLD = 0.5 - self._PARTIAL_FACE_THRESHOLD = 0.8 if TICI else 0.45 - self._EYE_THRESHOLD = 0.65 if TICI else 0.6 - self._SG_THRESHOLD = 0.925 if TICI else 0.91 - self._BLINK_THRESHOLD = 0.8 if TICI else 0.55 - self._BLINK_THRESHOLD_SLACK = 0.9 if TICI else 0.7 + self._PARTIAL_FACE_THRESHOLD = 0.8 + self._EYE_THRESHOLD = 0.65 + self._SG_THRESHOLD = 0.925 + self._BLINK_THRESHOLD = 0.8 + self._BLINK_THRESHOLD_SLACK = 0.9 self._BLINK_THRESHOLD_STRICT = self._BLINK_THRESHOLD - self._EE_THRESH11 = 0.75 if TICI else 0.4 - self._EE_THRESH12 = 3.25 if TICI else 2.45 + self._EE_THRESH11 = 0.75 + self._EE_THRESH12 = 3.25 self._EE_THRESH21 = 0.01 self._EE_THRESH22 = 0.35 diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 0690f1d8cf..1eda9bc7f5 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -242,14 +242,17 @@ CONFIGS = [ pub_sub={ "can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"], "deviceState": [], "pandaStates": [], "peripheralState": [], "liveCalibration": [], "driverMonitoringState": [], "longitudinalPlan": [], "lateralPlan": [], "liveLocationKalman": [], "liveParameters": [], "radarState": [], - "modelV2": [], "driverCameraState": [], "roadCameraState": [], "managerState": [], "testJoystick": [], + "modelV2": [], "driverCameraState": [], "roadCameraState": [], "wideRoadCameraState": [], "managerState": [], "testJoystick": [], }, ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"], init_callback=fingerprint, should_recv_callback=controlsd_rcv_callback, tolerance=NUMPY_TOLERANCE, fake_pubsubmaster=True, - submaster_config={'ignore_avg_freq': ['radarState', 'longitudinalPlan']} + submaster_config={ + 'ignore_avg_freq': ['radarState', 'longitudinalPlan', 'driverCameraState', 'driverMonitoringState'], # dcam is expected at 20 Hz + 'ignore_alive': ['wideRoadCameraState'], # TODO: Add to regen + } ), ProcessConfig( proc_name="radard", diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index de8599a58e..502fa15f2a 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -0956446adfa91506f0a3d88f893e041bfb2890c1 +49e231ccf06ef35994a3ec907af959c28e3c0870 \ No newline at end of file diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 76573e37e6..d5fd86279f 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -17,7 +17,7 @@ from common.filter_simple import FirstOrderFilter from common.params import Params from common.realtime import DT_TRML, sec_since_boot from selfdrive.controls.lib.alertmanager import set_offroad_alert -from selfdrive.hardware import HARDWARE, TICI +from selfdrive.hardware import HARDWARE, TICI, AGNOS from selfdrive.loggerd.config import get_available_percent from selfdrive.statsd import statlog from selfdrive.swaglog import cloudlog @@ -113,7 +113,7 @@ def hw_state_thread(end_event, hw_queue): modem_temps = prev_hw_state.modem_temps # Log modem version once - if TICI and ((modem_version is None) or (modem_nv is None)): + if AGNOS and ((modem_version is None) or (modem_nv is None)): modem_version = HARDWARE.get_modem_version() # pylint: disable=assignment-from-none modem_nv = HARDWARE.get_modem_nv() # pylint: disable=assignment-from-none @@ -134,7 +134,7 @@ def hw_state_thread(end_event, hw_queue): except queue.Full: pass - if TICI and (hw_state.network_info is not None) and (hw_state.network_info.get('state', None) == "REGISTERED"): + if AGNOS and (hw_state.network_info is not None) and (hw_state.network_info.get('state', None) == "REGISTERED"): registered_count += 1 else: registered_count = 0 diff --git a/selfdrive/timezoned.py b/selfdrive/timezoned.py index 6a7e9122c1..fc1ca92cdf 100755 --- a/selfdrive/timezoned.py +++ b/selfdrive/timezoned.py @@ -9,7 +9,7 @@ import requests from timezonefinder import TimezoneFinder from common.params import Params -from selfdrive.hardware import TICI +from selfdrive.hardware import AGNOS from selfdrive.swaglog import cloudlog @@ -20,7 +20,7 @@ def set_timezone(valid_timezones, timezone): cloudlog.debug(f"Setting timezone to {timezone}") try: - if TICI: + if AGNOS: tzpath = os.path.join("/usr/share/zoneinfo/", timezone) subprocess.check_call(f'sudo su -c "ln -snf {tzpath} /data/etc/tmptime && \ mv /data/etc/tmptime /data/etc/localtime"', shell=True) diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index dcf3ee0f67..1d0f8532db 100755 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -10,15 +10,12 @@ import glob from typing import NoReturn from common.file_helpers import mkdirs_exists_ok -from selfdrive.hardware import TICI from selfdrive.loggerd.config import ROOT import selfdrive.sentry as sentry from selfdrive.swaglog import cloudlog from selfdrive.version import get_commit -MAX_SIZE = 100000 * 10 # mal size is 40-100k, allow up to 1M -if TICI: - MAX_SIZE = MAX_SIZE * 100 # Allow larger size for tici since files contain coredump +MAX_SIZE = 1_000_000 * 100 # allow up to 100M MAX_TOMBSTONE_FN_LEN = 62 # 85 - 23 ("/crash/") TOMBSTONE_DIR = "/data/tombstones/" diff --git a/selfdrive/ui/qt/offroad/driverview.cc b/selfdrive/ui/qt/offroad/driverview.cc index 3302687bf4..06578b455a 100644 --- a/selfdrive/ui/qt/offroad/driverview.cc +++ b/selfdrive/ui/qt/offroad/driverview.cc @@ -63,12 +63,10 @@ void DriverViewScene::paintEvent(QPaintEvent* event) { // blackout const QColor bg(0, 0, 0, 140); - const QRect& blackout_rect = Hardware::TICI() ? rect() : rect2; + const QRect& blackout_rect = rect(); p.fillRect(blackout_rect.adjusted(0, 0, valid_rect.left() - blackout_rect.right(), 0), bg); p.fillRect(blackout_rect.adjusted(valid_rect.right() - blackout_rect.left(), 0, 0, 0), bg); - if (Hardware::TICI()) { - p.fillRect(blackout_rect.adjusted(valid_rect.left()-blackout_rect.left()+1, 0, valid_rect.right()-blackout_rect.right()-1, -valid_rect.height()*7/10), bg); // top dz - } + p.fillRect(blackout_rect.adjusted(valid_rect.left()-blackout_rect.left()+1, 0, valid_rect.right()-blackout_rect.right()-1, -valid_rect.height()*7/10), bg); // top dz // face bounding box cereal::DriverState::Reader driver_state = sm["driverState"].getDriverState(); diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 00e120d867..4aff797714 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -162,7 +162,7 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) { power_layout->addWidget(poweroff_btn); QObject::connect(poweroff_btn, &QPushButton::clicked, this, &DevicePanel::poweroff); - if (Hardware::TICI()) { + if (!Hardware::PC()) { connect(uiState(), &UIState::offroadTransition, poweroff_btn, &QPushButton::setVisible); } diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index eade9cbe30..f16e8e4e0d 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -55,25 +55,15 @@ const mat4 device_transform = {{ mat4 get_driver_view_transform(int screen_width, int screen_height, int stream_width, int stream_height) { const float driver_view_ratio = 1.333; - mat4 transform; - if (stream_width == TICI_CAM_WIDTH) { - const float yscale = stream_height * driver_view_ratio / tici_dm_crop::width; - const float xscale = yscale*screen_height/screen_width*stream_width/stream_height; - transform = (mat4){{ - xscale, 0.0, 0.0, xscale*tici_dm_crop::x_offset/stream_width*2, - 0.0, yscale, 0.0, yscale*tici_dm_crop::y_offset/stream_height*2, - 0.0, 0.0, 1.0, 0.0, - 0.0, 0.0, 0.0, 1.0, - }}; - } else { - // frame from 4/3 to 16/9 display - transform = (mat4){{ - driver_view_ratio * screen_height / screen_width, 0.0, 0.0, 0.0, - 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 1.0, 0.0, - 0.0, 0.0, 0.0, 1.0, - }}; - } + const float yscale = stream_height * driver_view_ratio / tici_dm_crop::width; + const float xscale = yscale*screen_height/screen_width*stream_width/stream_height; + mat4 transform = (mat4){{ + xscale, 0.0, 0.0, xscale*tici_dm_crop::x_offset/stream_width*2, + 0.0, yscale, 0.0, yscale*tici_dm_crop::y_offset/stream_height*2, + 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 1.0, + }}; + return transform; } diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index cd9bacf725..811438832b 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -182,7 +182,7 @@ static void update_state(UIState *s) { } } } - if (Hardware::TICI() && sm.updated("wideRoadCameraState")) { + if (sm.updated("wideRoadCameraState")) { auto camera_state = sm["wideRoadCameraState"].getWideRoadCameraState(); float max_lines = 1618; diff --git a/selfdrive/updated.py b/selfdrive/updated.py index 19dba9825e..f835522cdb 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -37,7 +37,7 @@ from markdown_it import MarkdownIt from common.basedir import BASEDIR from common.params import Params -from selfdrive.hardware import TICI, HARDWARE +from selfdrive.hardware import AGNOS, HARDWARE from selfdrive.swaglog import cloudlog from selfdrive.controls.lib.alertmanager import set_offroad_alert from selfdrive.version import is_tested_branch @@ -328,7 +328,7 @@ def fetch_update(wait_helper: WaitTimeHelper) -> bool: ] cloudlog.info("git reset success: %s", '\n'.join(r)) - if TICI: + if AGNOS: handle_agnos_update(wait_helper) # Create the finalized, ready-to-swap update From 302b0ea9bbac0fec939919b0d09b604569305be7 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 2 Jun 2022 13:39:41 -0700 Subject: [PATCH 29/29] thermald: add back panda disconnect handling (#24713) * thermald: add back panda disconnect handling * cleanup --- selfdrive/thermald/thermald.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index d5fd86279f..4df1e072b8 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -31,7 +31,7 @@ NetworkStrength = log.DeviceState.NetworkStrength CURRENT_TAU = 15. # 15s time constant TEMP_TAU = 5. # 5s time constant DISCONNECT_TIMEOUT = 5. # wait 5 seconds before going offroad after disconnect so you get an alert -PANDA_STATES_TIMEOUT = int(1000 * 2.5 * DT_TRML) # 2.5x the expected pandaState frequency +PANDA_STATES_TIMEOUT = int(1000 * 1.5 * DT_TRML) # 1.5x the expected pandaState frequency ThermalBand = namedtuple("ThermalBand", ['min_temp', 'max_temp']) HardwareState = namedtuple("HardwareState", ['network_type', 'network_metered', 'network_strength', 'network_info', 'nvme_temps', 'modem_temps']) @@ -220,6 +220,11 @@ def thermald_thread(end_event, hw_queue): if TICI: fan_controller = TiciFanController() + elif (sec_since_boot() - sm.rcv_time['pandaStates']/1e9) > DISCONNECT_TIMEOUT: + if onroad_conditions["ignition"]: + onroad_conditions["ignition"] = False + cloudlog.error("panda timed out onroad") + try: last_hw_state = hw_queue.get_nowait() except queue.Empty: