From 8d8055f00f11fcdf0a8a235de5218d9c2fd2f5ad Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Tue, 7 Jun 2022 12:25:58 +0200 Subject: [PATCH 01/93] Laikad: cleanup fetching orbits (#24759) * Seperate prediction orbits from regular observation orbits and download them efficient * Cleanup * clean and update laika * Fix test * Fix test * Fix checking pos fix * space --- laika_repo | 2 +- selfdrive/locationd/laikad.py | 26 ++++++++++--------------- selfdrive/locationd/test/test_laikad.py | 12 ++++++------ 3 files changed, 17 insertions(+), 23 deletions(-) diff --git a/laika_repo b/laika_repo index ba6ed3277c..d871946134 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit ba6ed3277cadfdc8697206784afbd7f9a223798b +Subproject commit d87194613455b42af19ff2b5a3f7d1cae5852885 diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index eee07e7805..d8e32005f8 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -6,11 +6,9 @@ from typing import List import numpy as np from collections import defaultdict -from numpy.linalg import linalg - from cereal import log, messaging from laika import AstroDog -from laika.constants import SECS_IN_HR, SECS_IN_MIN +from laika.constants import SECS_IN_MIN from laika.ephemeris import EphemerisType, convert_ublox_ephem from laika.gps_time import GPSTime from laika.helpers import ConstellationId @@ -26,10 +24,9 @@ MAX_TIME_GAP = 10 class Laikad: - def __init__(self, valid_const=("GPS",), auto_update=False, valid_ephem_types=(EphemerisType.ULTRA_RAPID_ORBIT, EphemerisType.NAV)): - self.astro_dog = AstroDog(valid_const=valid_const, use_internet=auto_update, valid_ephem_types=valid_ephem_types) + def __init__(self, valid_const=("GPS", "GLONASS"), auto_update=False, valid_ephem_types=(EphemerisType.ULTRA_RAPID_ORBIT, EphemerisType.NAV)): + self.astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types) self.gnss_kf = GNSSKalman(GENERATED_DIR) - self.latest_epoch_fetched = GPSTime(0, 0) self.latest_time_msg = None def process_ublox_msg(self, ublox_msg, ublox_mono_time: int): @@ -43,7 +40,7 @@ class Laikad: # To get a position fix a minimum of 5 measurements are needed. # Each report can contain less and some measurements can't be processed. corrected_measurements = [] - if len(pos_fix) > 0 and linalg.norm(pos_fix[1]) < 100: + if len(pos_fix) > 0 and abs(np.array(pos_fix[1])).mean() < 1000: corrected_measurements = correct_measurements(measurements, pos_fix[0][:3], self.astro_dog) t = ublox_mono_time * 1e-9 @@ -110,18 +107,15 @@ class Laikad: def orbit_thread(self, end_event: threading.Event): while not end_event.is_set(): if self.latest_time_msg: - self.fetch_orbits(self.latest_time_msg) + self.fetch_orbits(self.latest_time_msg + SECS_IN_MIN) time.sleep(0.1) def fetch_orbits(self, t: GPSTime): - if self.latest_epoch_fetched < t + SECS_IN_MIN: - cloudlog.info("Start to download/parse orbits") - orbit_ephems = self.astro_dog.download_parse_orbit_data(t, skip_before_epoch=t - 2 * SECS_IN_HR) - if len(orbit_ephems) > 0: - cloudlog.info(f"downloaded and parsed correctly new orbits {len(orbit_ephems)}, Constellations:{set([e.prn[0] for e in orbit_ephems])}") - self.astro_dog.add_ephems(orbit_ephems, self.astro_dog.orbits) - latest_orbit = max(orbit_ephems, key=lambda e: e.epoch) # type: ignore - self.latest_epoch_fetched = latest_orbit.epoch + if t not in self.astro_dog.orbit_fetched_times: + cloudlog.info(f"Start to download/parse orbits for time {t.as_datetime()}") + start_time = time.monotonic() + self.astro_dog.get_orbit_data(t, only_predictions=True) + cloudlog.info(f"Done parsing orbits. Took {time.monotonic() - start_time:.2f}s") def create_measurement_msg(meas: GNSSMeasurement): diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py index baa17792bd..8cb0773b37 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/selfdrive/locationd/test/test_laikad.py @@ -53,7 +53,6 @@ class TestLaikad(unittest.TestCase): def test_laika_online_nav_only(self): laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.NAV) correct_msgs = verify_messages(self.logs, laikad) - correct_msgs_expected = 560 self.assertEqual(correct_msgs_expected, len(correct_msgs)) self.assertEqual(correct_msgs_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) @@ -86,7 +85,7 @@ class TestLaikad(unittest.TestCase): break # Pretend thread has loaded the orbits on startup by using the time of the first gps message. laikad.fetch_orbits(first_gps_time) - self.assertEqual(31, len(laikad.astro_dog.orbits.keys())) + self.assertEqual(29, len(laikad.astro_dog.orbits.keys())) correct_msgs = verify_messages(self.logs, laikad) correct_msgs_expected = 560 self.assertEqual(correct_msgs_expected, len(correct_msgs)) @@ -96,10 +95,11 @@ class TestLaikad(unittest.TestCase): def test_laika_get_orbits_now(self): laikad = Laikad(auto_update=False) laikad.fetch_orbits(GPSTime.from_datetime(datetime.utcnow())) - print(laikad.latest_epoch_fetched.as_datetime()) - - print(min(laikad.astro_dog.orbits[list(laikad.astro_dog.orbits.keys())[0]], key=lambda e: e.epoch).epoch.as_datetime()) - + prn = "G01" + self.assertLess(0, len(laikad.astro_dog.orbits[prn])) + prn = "R01" + self.assertLess(0, len(laikad.astro_dog.orbits[prn])) + print(min(laikad.astro_dog.orbits[prn], key=lambda e: e.epoch).epoch.as_datetime()) if __name__ == "__main__": unittest.main() From b215d611b1adf5ea2b93e6628936f2e73539f34f Mon Sep 17 00:00:00 2001 From: ClockeNessMnstr Date: Tue, 7 Jun 2022 12:41:03 -0400 Subject: [PATCH 02/93] update DH names + notes for MPC output curvatures (#24701) * update names + notes for MPC outputs "current_curvature" is not the correct description of what the MPC is outputting in it's curvature_ego state. The MPC is integrating it's free variable, curvature_rate, such that curvature[0] is actually the desired_curvature before any delay. inversely: the curvature_rate_desired is the desired rate of change to the setpoint and not the actual curvature rate. If we were to set the initial curvature = measured curvature in the MPC initiation these names would be correct. This was possibly how it was initially set up but the nomenclature here is now confusing. * more notes * match * Clarify #1 --- selfdrive/controls/lib/drive_helpers.py | 23 +++++++++++---------- selfdrive/controls/lib/latcontrol_indi.py | 1 + selfdrive/controls/lib/latcontrol_torque.py | 2 ++ selfdrive/controls/lib/lateral_planner.py | 3 +++ 4 files changed, 18 insertions(+), 11 deletions(-) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 4afa8d89ed..91981259aa 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -97,26 +97,27 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates): psis = [0.0]*CONTROL_N curvatures = [0.0]*CONTROL_N curvature_rates = [0.0]*CONTROL_N - + v_ego = max(v_ego, 0.1) + # TODO this needs more thought, use .2s extra for now to estimate other delays delay = CP.steerActuatorDelay + .2 - current_curvature = curvatures[0] - psi = interp(delay, T_IDXS[:CONTROL_N], psis) - desired_curvature_rate = curvature_rates[0] - + # MPC can plan to turn the wheel and turn back before t_delay. This means # in high delay cases some corrections never even get commanded. So just use # psi to calculate a simple linearization of desired curvature - curvature_diff_from_psi = psi / (max(v_ego, 1e-1) * delay) - current_curvature - desired_curvature = current_curvature + 2 * curvature_diff_from_psi - - v_ego = max(v_ego, 0.1) + current_curvature_desired = curvatures[0] + psi = interp(delay, T_IDXS[:CONTROL_N], psis) + average_curvature_desired = psi / (v_ego * delay) + desired_curvature = 2 * average_curvature_desired - current_curvature_desired + + # This is the "desired rate of the setpoint" not an actual desired rate + desired_curvature_rate = curvature_rates[0] max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) safe_desired_curvature_rate = clip(desired_curvature_rate, -max_curvature_rate, max_curvature_rate) safe_desired_curvature = clip(desired_curvature, - current_curvature - max_curvature_rate * DT_MDL, - current_curvature + max_curvature_rate * DT_MDL) + current_curvature_desired - max_curvature_rate * DT_MDL, + current_curvature_desired + max_curvature_rate * DT_MDL) return safe_desired_curvature, safe_desired_curvature_rate diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index b5041eb172..79c881d11b 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -78,6 +78,7 @@ class LatControlINDI(LatControl): steers_des += math.radians(params.angleOffsetDeg) indi_log.steeringAngleDesiredDeg = math.degrees(steers_des) + # desired rate is the desired rate of change in the setpoint, not the absolute desired curvature rate_des = VM.get_steer_from_curvature(-desired_curvature_rate, CS.vEgo, 0) indi_log.steeringRateDesiredDeg = math.degrees(rate_des) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 8b41dbf402..47f0b69f2b 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -59,6 +59,8 @@ class LatControlTorque(LatControl): actual_curvature_llk = llk.angularVelocityCalibrated.value[2] / CS.vEgo actual_curvature = interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_llk]) desired_lateral_accel = desired_curvature * CS.vEgo ** 2 + + # desired rate is the desired rate of change in the setpoint, not the absolute desired curvature desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2 actual_lateral_accel = actual_curvature * CS.vEgo ** 2 diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 6349f05cc2..f3f59ee308 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -79,6 +79,9 @@ class LateralPlanner: y_pts, heading_pts) # init state for next + # mpc.u_sol is the desired curvature rate given x0 curv state. + # with x0[3] = measured_curvature, this would be the actual desired rate. + # instead, interpolate x_sol so that x0[3] is the desired curvature for lat_control. self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3]) # Check for infeasible MPC solution From e7234e22b4d11cc8f349d02855df9b3414096c0d Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Tue, 7 Jun 2022 20:55:39 +0200 Subject: [PATCH 03/93] Laikad: Use process for parsing orbits (#24769) * Use Process instead of Thread to fetch orbits * small refactor * Cleanup --- selfdrive/locationd/laikad.py | 66 ++++++++++++++----------- selfdrive/locationd/test/test_laikad.py | 10 ++-- 2 files changed, 39 insertions(+), 37 deletions(-) diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index d8e32005f8..f0c52e7e03 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 -import threading import time -from typing import List +from multiprocessing import Process, Queue +from typing import List, Optional import numpy as np from collections import defaultdict @@ -27,14 +27,17 @@ class Laikad: def __init__(self, valid_const=("GPS", "GLONASS"), auto_update=False, valid_ephem_types=(EphemerisType.ULTRA_RAPID_ORBIT, EphemerisType.NAV)): self.astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types) self.gnss_kf = GNSSKalman(GENERATED_DIR) - self.latest_time_msg = None + self.orbit_p: Optional[Process] = None + self.orbit_q = Queue() def process_ublox_msg(self, ublox_msg, ublox_mono_time: int): if ublox_msg.which == 'measurementReport': report = ublox_msg.measurementReport - new_meas = read_raw_ublox(report) if report.gpsWeek > 0: - self.latest_time_msg = GPSTime(report.gpsWeek, report.rcvTow) + latest_msg_t = GPSTime(report.gpsWeek, report.rcvTow) + self.fetch_orbits(latest_msg_t + SECS_IN_MIN, block=False) + new_meas = read_raw_ublox(report) + measurements = process_measurements(new_meas, self.astro_dog) pos_fix = calc_pos_fix(measurements, min_measurements=4) # To get a position fix a minimum of 5 measurements are needed. @@ -104,18 +107,28 @@ class Laikad: self.gnss_kf.init_state(x_initial, covs_diag=p_initial_diag) - def orbit_thread(self, end_event: threading.Event): - while not end_event.is_set(): - if self.latest_time_msg: - self.fetch_orbits(self.latest_time_msg + SECS_IN_MIN) - time.sleep(0.1) + def get_orbit_data(self, t: GPSTime, queue): + cloudlog.info(f"Start to download/parse orbits for time {t.as_datetime()}") + start_time = time.monotonic() + self.astro_dog.get_orbit_data(t, only_predictions=True) + cloudlog.info(f"Done parsing orbits. Took {time.monotonic() - start_time:.2f}s") + queue.put((self.astro_dog.orbits, self.astro_dog.orbit_fetched_times)) - def fetch_orbits(self, t: GPSTime): + def fetch_orbits(self, t: GPSTime, block): if t not in self.astro_dog.orbit_fetched_times: - cloudlog.info(f"Start to download/parse orbits for time {t.as_datetime()}") - start_time = time.monotonic() - self.astro_dog.get_orbit_data(t, only_predictions=True) - cloudlog.info(f"Done parsing orbits. Took {time.monotonic() - start_time:.2f}s") + if self.orbit_p is None: + self.orbit_p = Process(target=self.get_orbit_data, args=(t, self.orbit_q)) + self.orbit_p.start() + ret = None + if block: + ret = self.orbit_q.get(block=True) + elif not self.orbit_q.empty(): + ret = self.orbit_q.get() + + if ret: + self.astro_dog.orbits, self.astro_dog.orbit_fetched_times = ret + self.orbit_p.join() + self.orbit_p = None def create_measurement_msg(meas: GNSSMeasurement): @@ -162,21 +175,14 @@ def main(): pm = messaging.PubMaster(['gnssMeasurements']) laikad = Laikad() - - end_event = threading.Event() - threading.Thread(target=laikad.orbit_thread, args=(end_event,)).start() - try: - while not end_event.is_set(): - sm.update() - - if sm.updated['ubloxGnss']: - ublox_msg = sm['ubloxGnss'] - msg = laikad.process_ublox_msg(ublox_msg, sm.logMonoTime['ubloxGnss']) - if msg is not None: - pm.send('gnssMeasurements', msg) - except (KeyboardInterrupt, SystemExit): - end_event.set() - raise + while True: + sm.update() + + if sm.updated['ubloxGnss']: + ublox_msg = sm['ubloxGnss'] + msg = laikad.process_ublox_msg(ublox_msg, sm.logMonoTime['ubloxGnss']) + if msg is not None: + pm.send('gnssMeasurements', msg) if __name__ == "__main__": diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py index 8cb0773b37..630a7525eb 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/selfdrive/locationd/test/test_laikad.py @@ -83,18 +83,14 @@ class TestLaikad(unittest.TestCase): if len(new_meas) != 0: first_gps_time = new_meas[0].recv_time break - # Pretend thread has loaded the orbits on startup by using the time of the first gps message. - laikad.fetch_orbits(first_gps_time) + # Pretend process has loaded the orbits on startup by using the time of the first gps message. + laikad.fetch_orbits(first_gps_time, block=True) self.assertEqual(29, len(laikad.astro_dog.orbits.keys())) - correct_msgs = verify_messages(self.logs, laikad) - correct_msgs_expected = 560 - self.assertEqual(correct_msgs_expected, len(correct_msgs)) - self.assertEqual(correct_msgs_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) @unittest.skip("Use to debug live data") def test_laika_get_orbits_now(self): laikad = Laikad(auto_update=False) - laikad.fetch_orbits(GPSTime.from_datetime(datetime.utcnow())) + laikad.fetch_orbits(GPSTime.from_datetime(datetime.utcnow()), block=True) prn = "G01" self.assertLess(0, len(laikad.astro_dog.orbits[prn])) prn = "R01" From 4d836c6edd6e0f2639a3e2500794990da339eddc Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Tue, 7 Jun 2022 13:59:48 -0700 Subject: [PATCH 04/93] UI: remove DM icon (#24771) --- selfdrive/ui/qt/onroad.cc | 10 +--------- selfdrive/ui/qt/onroad.h | 5 ----- selfdrive/ui/ui.cc | 2 +- 3 files changed, 2 insertions(+), 15 deletions(-) diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index ce61c094bd..e549f62a23 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -166,7 +166,6 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) { NvgWindow::NvgWindow(VisionStreamType type, QWidget* parent) : fps_filter(UI_FREQ, 3, 1. / UI_FREQ), CameraViewWidget("camerad", type, true, parent) { engage_img = loadPixmap("../assets/img_chffr_wheel.png", {img_size, img_size}); - dm_img = loadPixmap("../assets/img_driver_face.png", {img_size, img_size}); } void NvgWindow::updateState(const UIState &s) { @@ -186,13 +185,11 @@ void NvgWindow::updateState(const UIState &s) { setProperty("speed", QString::number(std::nearbyint(cur_speed))); setProperty("maxSpeed", maxspeed_str); setProperty("speedUnit", s.scene.is_metric ? "km/h" : "mph"); - setProperty("hideDM", cs.getAlertSize() != cereal::ControlsState::AlertSize::NONE); setProperty("status", s.status); - // update engageability and DM icons at 2Hz + // update engageability at 2Hz if (sm.frame % (UI_FREQ / 2) == 0) { setProperty("engageable", cs.getEngageable() || cs.getEnabled()); - setProperty("dmActive", sm["driverMonitoringState"].getDriverMonitoringState().getIsActiveMode()); } } @@ -234,11 +231,6 @@ void NvgWindow::drawHud(QPainter &p) { engage_img, bg_colors[status], 1.0); } - // dm icon - if (!hideDM) { - drawIcon(p, radius / 2 + (bdr_s * 2), rect().bottom() - footer_h / 2, - dm_img, QColor(0, 0, 0, 70), dmActive ? 1.0 : 0.2); - } p.restore(); } diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 6ca2b3c738..e1f665149e 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -32,8 +32,6 @@ class NvgWindow : public CameraViewWidget { Q_PROPERTY(QString maxSpeed MEMBER maxSpeed); Q_PROPERTY(bool is_cruise_set MEMBER is_cruise_set); Q_PROPERTY(bool engageable MEMBER engageable); - Q_PROPERTY(bool dmActive MEMBER dmActive); - Q_PROPERTY(bool hideDM MEMBER hideDM); Q_PROPERTY(int status MEMBER status); public: @@ -45,7 +43,6 @@ private: void drawText(QPainter &p, int x, int y, const QString &text, int alpha = 255); QPixmap engage_img; - QPixmap dm_img; const int radius = 192; const int img_size = (radius / 2) * 1.5; QString speed; @@ -53,8 +50,6 @@ private: QString maxSpeed; bool is_cruise_set = false; bool engageable = false; - bool dmActive = false; - bool hideDM = false; int status = STATUS_DISENGAGED; protected: diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index c1af4ed6f4..f7c2c90437 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -231,7 +231,7 @@ void UIState::updateStatus() { UIState::UIState(QObject *parent) : QObject(parent) { sm = std::make_unique>({ "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "roadCameraState", - "pandaStates", "carParams", "driverMonitoringState", "sensorEvents", "carState", "liveLocationKalman", + "pandaStates", "carParams", "sensorEvents", "carState", "liveLocationKalman", "wideRoadCameraState", "managerState", "navInstruction", "navRoute", }); From 6a58dd808f3e168fe72129b09ca5f829a4c146db Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 7 Jun 2022 15:01:19 -0700 Subject: [PATCH 05/93] PlotJuggler: add torque control layout (#24726) * add torque control PJ layout * less custom transformation * Use curvature, less noisy * remove that --- .../plotjuggler/layouts/torque-controller.xml | 88 +++++++++++++++++++ 1 file changed, 88 insertions(+) create mode 100644 tools/plotjuggler/layouts/torque-controller.xml diff --git a/tools/plotjuggler/layouts/torque-controller.xml b/tools/plotjuggler/layouts/torque-controller.xml new file mode 100644 index 0000000000..661bfe094d --- /dev/null +++ b/tools/plotjuggler/layouts/torque-controller.xml @@ -0,0 +1,88 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + return (value * v1 ^ 2) - (v2 * 9.81) + /controlsState/curvature + + /carState/vEgo + /liveParameters/roll + + + + + return (value * v1 ^ 2) - (v2 * 9.81) + /controlsState/desiredCurvature + + /carState/vEgo + /liveParameters/roll + + + + + + From bdc05fd13cdbdb4879fbadf5d5262bd8236b3243 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 7 Jun 2022 17:18:03 -0700 Subject: [PATCH 06/93] Honda: rename Bosch harness to Bosch A --- selfdrive/car/docs_definitions.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/docs_definitions.py b/selfdrive/car/docs_definitions.py index 381bcfa7f8..95561872ac 100644 --- a/selfdrive/car/docs_definitions.py +++ b/selfdrive/car/docs_definitions.py @@ -120,7 +120,7 @@ class CarInfo: class Harness(Enum): nidec = "Honda Nidec" - bosch = "Honda Bosch" + bosch = "Honda Bosch A" toyota = "Toyota" subaru = "Subaru" fca = "FCA" From b95c449847da37809ab3cd9a582cd910a612a1cf Mon Sep 17 00:00:00 2001 From: Jason Shuler Date: Tue, 7 Jun 2022 20:23:08 -0400 Subject: [PATCH 07/93] GM: interface cleanup (#24774) * Move all defaults above models * Remove reduntant/defaults * make minEnableSpeed common * Update selfdrive/car/gm/interface.py Co-authored-by: Adeeb Shihadeh Co-authored-by: Shane Smiskol Co-authored-by: Adeeb Shihadeh --- selfdrive/car/gm/interface.py | 36 +++++++++++++---------------------- 1 file changed, 13 insertions(+), 23 deletions(-) diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index ce14faec5e..bc43086586 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -58,7 +58,7 @@ class CarInterface(CarInterfaceBase): ret.openpilotLongitudinalControl = True tire_stiffness_factor = 0.444 # not optimized yet - # Start with a baseline lateral tuning for all GM vehicles. Override tuning as needed in each model section below. + # Start with a baseline tuning for all GM vehicles. Override tuning as needed in each model section below. ret.minSteerSpeed = 7 * CV.MPH_TO_MS ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]] @@ -66,14 +66,22 @@ class CarInterface(CarInterfaceBase): ret.steerRateCost = 1.0 ret.steerActuatorDelay = 0.1 # Default delay, not measured yet + ret.longitudinalTuning.kpBP = [5., 35.] + ret.longitudinalTuning.kpV = [2.4, 1.5] + ret.longitudinalTuning.kiBP = [0.] + ret.longitudinalTuning.kiV = [0.36] + + ret.steerLimitTimer = 0.4 + ret.radarTimeStep = 0.0667 # GM radar runs at 15Hz instead of standard 20Hz + + # supports stop and go, but initial engage must (conservatively) be above 18mph + ret.minEnableSpeed = 18 * CV.MPH_TO_MS + if candidate == CAR.VOLT: - # supports stop and go, but initial engage must be above 18mph (which include conservatism) - ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.mass = 1607. + STD_CARGO_KG ret.wheelbase = 2.69 ret.steerRatio = 17.7 # Stock 15.7, LiveParameters tire_stiffness_factor = 0.469 # Stock Michelin Energy Saver A/S, LiveParameters - ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.45 # Volt Gen 1, TODO corner weigh ret.maxLateralAccel = 1.6 @@ -85,12 +93,9 @@ class CarInterface(CarInterfaceBase): ret.steerActuatorDelay = 0.2 elif candidate == CAR.MALIBU: - # supports stop and go, but initial engage must be above 18mph (which include conservatism) - ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.mass = 1496. + STD_CARGO_KG ret.wheelbase = 2.83 ret.steerRatio = 15.8 - ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.4 # wild guess elif candidate == CAR.HOLDEN_ASTRA: @@ -98,34 +103,27 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.662 # Remaining parameters copied from Volt for now ret.centerToFront = ret.wheelbase * 0.4 - ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.steerRatio = 15.7 - ret.steerRatioRear = 0. elif candidate == CAR.ACADIA: ret.minEnableSpeed = -1. # engage speed is decided by pcm ret.mass = 4353. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.86 ret.steerRatio = 14.4 # end to end is 13.46 - ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.4 ret.maxLateralAccel = 1.4 - ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_acadia() + ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_acadia() elif candidate == CAR.BUICK_REGAL: - ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.mass = 3779. * CV.LB_TO_KG + STD_CARGO_KG # (3849+3708)/2 ret.wheelbase = 2.83 # 111.4 inches in meters ret.steerRatio = 14.4 # guess for tourx - ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.4 # guess for tourx elif candidate == CAR.CADILLAC_ATS: - ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.mass = 1601. + STD_CARGO_KG ret.wheelbase = 2.78 ret.steerRatio = 15.3 - ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.49 elif candidate == CAR.ESCALADE_ESV: @@ -148,14 +146,6 @@ class CarInterface(CarInterfaceBase): ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, tire_stiffness_factor=tire_stiffness_factor) - ret.longitudinalTuning.kpBP = [5., 35.] - ret.longitudinalTuning.kpV = [2.4, 1.5] - ret.longitudinalTuning.kiBP = [0.] - ret.longitudinalTuning.kiV = [0.36] - - ret.steerLimitTimer = 0.4 - ret.radarTimeStep = 0.0667 # GM radar runs at 15Hz instead of standard 20Hz - return ret # returns a car.CarState From 30e1f282134f7693a451e783b1d2d8ee68a79c06 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 7 Jun 2022 17:49:07 -0700 Subject: [PATCH 08/93] IsoTpParallelQuery: handle response pending (#24724) * handle response pending * match commit * remove total timeout, just keep track of individual response timeouts * fix * add back total timeout * this isn't reliable enough * keep track of pending responses to print warning * tx_addr is (addr, subaddr) * debug only query hyundai import time reponse pending no cache all cars no timeout to test before * Revert "debug" This reverts commit abe9cfc1b668034d7fa5ca5cbe9efe8834db3e7b. * always print pending always debug * Only debug * Update selfdrive/car/isotp_parallel_query.py * remove variable only for debugging --- selfdrive/car/isotp_parallel_query.py | 26 +++++++++++++++----------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/selfdrive/car/isotp_parallel_query.py b/selfdrive/car/isotp_parallel_query.py index becd66ba1f..dc79babb13 100644 --- a/selfdrive/car/isotp_parallel_query.py +++ b/selfdrive/car/isotp_parallel_query.py @@ -10,7 +10,7 @@ from panda.python.uds import CanClient, IsoTpMessage, FUNCTIONAL_ADDRS, get_rx_a class IsoTpParallelQuery: - def __init__(self, sendcan, logcan, bus, addrs, request, response, response_offset=0x8, functional_addr=False, debug=False): + def __init__(self, sendcan, logcan, bus, addrs, request, response, response_offset=0x8, functional_addr=False, debug=False, response_pending_timeout=10): self.sendcan = sendcan self.logcan = logcan self.bus = bus @@ -18,6 +18,7 @@ class IsoTpParallelQuery: self.response = response self.debug = debug self.functional_addr = functional_addr + self.response_pending_timeout = response_pending_timeout self.real_addrs = [] for a in addrs: @@ -71,10 +72,7 @@ class IsoTpParallelQuery: messaging.drain_sock(self.logcan) self.msg_buffer = defaultdict(list) - def get_data(self, timeout, total_timeout=None): - if total_timeout is None: - total_timeout = 10 * timeout - + def get_data(self, timeout, total_timeout=60.): self._drain_rx() # Create message objects @@ -100,7 +98,7 @@ class IsoTpParallelQuery: results = {} start_time = time.monotonic() - last_response_time = start_time + response_timeouts = {tx_addr: start_time + timeout for tx_addr in self.msg_addrs} while True: self.rx() @@ -123,7 +121,7 @@ class IsoTpParallelQuery: response_valid = dat[:len(expected_response)] == expected_response if response_valid: - last_response_time = time.monotonic() + response_timeouts[tx_addr] = time.monotonic() + timeout if counter + 1 < len(self.request): msg.send(self.request[counter + 1]) request_counter[tx_addr] += 1 @@ -131,13 +129,19 @@ class IsoTpParallelQuery: results[tx_addr] = dat[len(expected_response):] request_done[tx_addr] = True else: - request_done[tx_addr] = True - cloudlog.warning(f"iso-tp query bad response: {tx_addr} - 0x{dat.hex()}") + error_code = dat[2] if len(dat) > 2 else -1 + if error_code == 0x78: + response_timeouts[tx_addr] = time.monotonic() + self.response_pending_timeout + if self.debug: + cloudlog.warning(f"iso-tp query response pending: {tx_addr}") + else: + request_done[tx_addr] = True + cloudlog.warning(f"iso-tp query bad response: {tx_addr} - 0x{dat.hex()}") cur_time = time.monotonic() - if cur_time - last_response_time > timeout: + if cur_time - max(response_timeouts.values()) > 0: for tx_addr in msgs: - if (request_counter[tx_addr] > 0) and (not request_done[tx_addr]): + if request_counter[tx_addr] > 0 and not request_done[tx_addr]: cloudlog.warning(f"iso-tp query timeout after receiving response: {tx_addr}") break From afe208ecb996d10cda598fe2f2fade7353d420cf Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Tue, 7 Jun 2022 19:16:36 -0700 Subject: [PATCH 09/93] Install readme (#24776) README: better install on device instructions --- README.md | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 9d279fb84d..c426b839d5 100755 --- a/README.md +++ b/README.md @@ -35,16 +35,17 @@ What is openpilot? -Running in a car +Running on a dedicated device in a car ------ To use openpilot in a car, you need four things -* This software. It's free and available right here. +* A supported device to run this software: a [comma three](https://comma.ai/shop/products/three). +* This software. The setup procedure of the comma three allows the user to enter a url for custom software. +The url, openpilot.comma.ai will install the release version of openpilot. To install openpilot master, you can use installer.comma.ai/commaai/master, and replacing commaai with another github username can install a fork. * One of [the 150+ supported cars](docs/CARS.md). We support Honda, Toyota, Hyundai, Nissan, Kia, Chrysler, Lexus, Acura, Audi, VW, and more. If your car is not supported, but has adaptive cruise control and lane keeping assist, it's likely able to run openpilot. -* A supported device to run this software: a [comma three](https://comma.ai/shop/products/three), or if you like to experiment, a [Ubuntu computer with webcams](https://github.com/commaai/openpilot/tree/master/tools/webcam). -* A way to connect to your car. With a comma three, you need only a [car harness](https://comma.ai/shop/products/car-harness). With a PC, you also need a [black panda](https://comma.ai/shop/products/panda). +* A [car harness](https://comma.ai/shop/products/car-harness) to connect to your car. -We have detailed instructions for [how to install the device in a car](https://comma.ai/setup). +We have detailed instructions for [how to mount the device in a car](https://comma.ai/setup). Running on PC ------ @@ -55,6 +56,7 @@ With openpilot's tools you can plot logs, replay drives and watch the full-res c You can also run openpilot in simulation [with the CARLA simulator](tools/sim/README.md). This allows openpilot to drive around a virtual car on your Ubuntu machine. The whole setup should only take a few minutes, but does require a decent GPU. +A PC running openpilot can also control your vehicle if it is connected to a [a webcam](https://github.com/commaai/openpilot/tree/master/tools/webcam), a [black panda](https://comma.ai/shop/products/panda), and [a harness](https://comma.ai/shop/products/car-harness). Community and Contributing ------ From ce6b8f6a461d864d52ef8c6a00466d951aecbdd6 Mon Sep 17 00:00:00 2001 From: Jason Young <46612682+jyoung8607@users.noreply.github.com> Date: Wed, 8 Jun 2022 00:57:15 -0400 Subject: [PATCH 10/93] VW MQB: Add FW for 2020 Volkswagen Tiguan (#24777) --- selfdrive/car/volkswagen/values.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index 44743c948e..8cba9988bd 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -533,6 +533,7 @@ FW_VERSIONS = { }, CAR.TIGUAN_MK2: { (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704E906027NB\xf1\x899504', b'\xf1\x8704L906026EJ\xf1\x893661', b'\xf1\x8704L906027G \xf1\x899893', b'\xf1\x875N0906259 \xf1\x890002', @@ -544,6 +545,7 @@ FW_VERSIONS = { b'\xf1\x8709G927158DT\xf1\x893698', b'\xf1\x8709G927158GC\xf1\x893821', b'\xf1\x8709G927158GD\xf1\x893820', + b'\xf1\x870D9300043 \xf1\x895202', b'\xf1\x870DL300011N \xf1\x892001', b'\xf1\x870DL300011N \xf1\x892012', b'\xf1\x870DL300013A \xf1\x893005', @@ -555,11 +557,13 @@ FW_VERSIONS = { b'\xf1\x875Q0959655BM\xf1\x890403\xf1\x82\02316143231313500314641011750179333423100', b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\02312110031333300314240583752379333423100', b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\02331310031333336313140013950399333423100', + b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x1331310031333334313140013750379333423100', b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x1331310031333334313140573752379333423100', b'\xf1\x875Q0959655CB\xf1\x890421\xf1\x82\x1316143231313500314647021750179333613100', ], (Ecu.eps, 0x712, None): [ b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820529A6060603', + b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\x0521A60604A1', b'\xf1\x875QF909144B \xf1\x895582\xf1\x82\00571A60634A1', b'\xf1\x875QM909144B \xf1\x891081\xf1\x82\x0521A60604A1', b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\x0521A60604A1', From abef7ca32cac34fd15cb17e1fafb96e961486ba3 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 7 Jun 2022 22:43:09 -0700 Subject: [PATCH 11/93] Kia Ceed: fix eps ECU type (#24767) should be eps --- selfdrive/car/hyundai/values.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 1cd2acabeb..54bbd80475 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -845,7 +845,7 @@ FW_VERSIONS = { }, CAR.KIA_CEED: { (Ecu.fwdRadar, 0x7D0, None): [b'\xf1\000CD__ SCC F-CUP 1.00 1.02 99110-J7000 ', ], - (Ecu.esp, 0x7D4, None): [b'\xf1\000CD MDPS C 1.00 1.06 56310-XX000 4CDEC106', ], + (Ecu.eps, 0x7D4, None): [b'\xf1\000CD MDPS C 1.00 1.06 56310-XX000 4CDEC106', ], (Ecu.fwdCamera, 0x7C4, None): [b'\xf1\000CD LKAS AT EUR LHD 1.00 1.01 99211-J7000 B40', ], (Ecu.engine, 0x7E0, None): [b'\001TCD-JECU4F202H0K', ], (Ecu.transmission, 0x7E1, None): [ From c8db61a3c7aba6f96c9f282f4984c84fd6550f62 Mon Sep 17 00:00:00 2001 From: Erich Moraga <33645296+ErichMoraga@users.noreply.github.com> Date: Wed, 8 Jun 2022 00:47:10 -0500 Subject: [PATCH 12/93] Add missing MIRAI ESP f/w (#24779) * Add missing ESP f/w `@JaySheikh#7759` 2021 Mirai DongleID/route b2c2b20082938bed|2022-06-08--05-04-03 [confirmed working] * formatting Co-authored-by: Shane Smiskol --- selfdrive/car/toyota/values.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 3204d36b5a..a9517d26bf 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -1825,7 +1825,10 @@ FW_VERSIONS = { }, CAR.MIRAI: { (Ecu.esp, 0x7D1, None): [b'\x01898A36203000\x00\x00\x00\x00',], - (Ecu.esp, 0x7B0, None): [b'\x01F15266203200\x00\x00\x00\x00',], # a second ESP ECU + (Ecu.esp, 0x7B0, None): [ # a second ESP ECU + b'\x01F15266203200\x00\x00\x00\x00', + b'\x01F15266203500\x00\x00\x00\x00', + ], (Ecu.eps, 0x7A1, None): [b'\x028965B6204100\x00\x00\x00\x008965B6203100\x00\x00\x00\x00',], (Ecu.fwdRadar, 0x750, 0xf): [b'\x018821F6201200\x00\x00\x00\x00',], (Ecu.fwdCamera, 0x750, 0x6d): [b'\x028646F6201400\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',], From 45c43f9c8719efc300a0cb373f1dc57e0abe3ece Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Wed, 8 Jun 2022 12:32:22 +0200 Subject: [PATCH 13/93] Laikad: kill process correctly. Fixes CI (#24780) Kill process correctly --- selfdrive/locationd/laikad.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index f0c52e7e03..a55302458e 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -130,6 +130,10 @@ class Laikad: self.orbit_p.join() self.orbit_p = None + def __del__(self): + if self.orbit_p is not None: + self.orbit_p.kill() + def create_measurement_msg(meas: GNSSMeasurement): c = log.GnssMeasurements.CorrectedMeasurement.new_message() From a9bdc792a184d191824d2ec089658eb3a96f1e45 Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Wed, 8 Jun 2022 16:33:05 +0200 Subject: [PATCH 14/93] LaikaD: Fix offline handling (#24781) * Test handling no internet correctly. * Clean * Comment * remove del --- selfdrive/locationd/laikad.py | 30 ++++++++++++++----------- selfdrive/locationd/test/test_laikad.py | 22 ++++++++---------- 2 files changed, 26 insertions(+), 26 deletions(-) diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index a55302458e..4ec159cd25 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -30,12 +30,12 @@ class Laikad: self.orbit_p: Optional[Process] = None self.orbit_q = Queue() - def process_ublox_msg(self, ublox_msg, ublox_mono_time: int): + def process_ublox_msg(self, ublox_msg, ublox_mono_time: int, block=False): if ublox_msg.which == 'measurementReport': report = ublox_msg.measurementReport if report.gpsWeek > 0: latest_msg_t = GPSTime(report.gpsWeek, report.rcvTow) - self.fetch_orbits(latest_msg_t + SECS_IN_MIN, block=False) + self.fetch_orbits(latest_msg_t + SECS_IN_MIN, block) new_meas = read_raw_ublox(report) measurements = process_measurements(new_meas, self.astro_dog) @@ -110,25 +110,29 @@ class Laikad: def get_orbit_data(self, t: GPSTime, queue): cloudlog.info(f"Start to download/parse orbits for time {t.as_datetime()}") start_time = time.monotonic() - self.astro_dog.get_orbit_data(t, only_predictions=True) + try: + self.astro_dog.get_orbit_data(t, only_predictions=True) + except RuntimeError as e: + cloudlog.info(f"No orbit data found. {e}") + return cloudlog.info(f"Done parsing orbits. Took {time.monotonic() - start_time:.2f}s") - queue.put((self.astro_dog.orbits, self.astro_dog.orbit_fetched_times)) + if queue is not None: + queue.put((self.astro_dog.orbits, self.astro_dog.orbit_fetched_times)) def fetch_orbits(self, t: GPSTime, block): if t not in self.astro_dog.orbit_fetched_times: + if block: + self.get_orbit_data(t, None) + return if self.orbit_p is None: self.orbit_p = Process(target=self.get_orbit_data, args=(t, self.orbit_q)) self.orbit_p.start() - ret = None - if block: - ret = self.orbit_q.get(block=True) - elif not self.orbit_q.empty(): + if not self.orbit_q.empty(): ret = self.orbit_q.get() - - if ret: - self.astro_dog.orbits, self.astro_dog.orbit_fetched_times = ret - self.orbit_p.join() - self.orbit_p = None + if ret: + self.astro_dog.orbits, self.astro_dog.orbit_fetched_times = ret + self.orbit_p.join() + self.orbit_p = None def __del__(self): if self.orbit_p is not None: diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py index 630a7525eb..7a8c1ecb13 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/selfdrive/locationd/test/test_laikad.py @@ -1,6 +1,8 @@ #!/usr/bin/env python3 import unittest from datetime import datetime +from unittest import mock +from unittest.mock import Mock from laika.ephemeris import EphemerisType from laika.gps_time import GPSTime @@ -21,7 +23,7 @@ def get_log(segs=range(0)): def verify_messages(lr, laikad): good_msgs = [] for m in lr: - msg = laikad.process_ublox_msg(m.ubloxGnss, m.logMonoTime) + msg = laikad.process_ublox_msg(m.ubloxGnss, m.logMonoTime, block=True) if msg is not None and len(msg.gnssMeasurements.correctedMeasurements) > 0: good_msgs.append(msg) return good_msgs @@ -52,28 +54,21 @@ class TestLaikad(unittest.TestCase): def test_laika_online_nav_only(self): laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.NAV) + # Disable fetch_orbits to test NAV only + laikad.fetch_orbits = Mock() correct_msgs = verify_messages(self.logs, laikad) correct_msgs_expected = 560 self.assertEqual(correct_msgs_expected, len(correct_msgs)) self.assertEqual(correct_msgs_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) - def test_laika_offline(self): - # Set auto_update to false forces to use ephemeris messages + @mock.patch('laika.downloader.download_and_cache_file') + def test_laika_offline(self, downloader_mock): + downloader_mock.side_effect = IOError laikad = Laikad(auto_update=False) correct_msgs = verify_messages(self.logs, laikad) - self.assertEqual(256, len(correct_msgs)) self.assertEqual(256, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) - def test_laika_offline_ephem_at_start(self): - # Test offline but process ephemeris msgs of segment first - laikad = Laikad(auto_update=False, valid_ephem_types=EphemerisType.NAV) - ephemeris_logs = [m for m in self.logs if m.ubloxGnss.which() == 'ephemeris'] - correct_msgs = verify_messages(ephemeris_logs+self.logs, laikad) - - self.assertEqual(554, len(correct_msgs)) - self.assertGreaterEqual(554, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) - def test_laika_get_orbits(self): laikad = Laikad(auto_update=False) first_gps_time = None @@ -97,5 +92,6 @@ class TestLaikad(unittest.TestCase): self.assertLess(0, len(laikad.astro_dog.orbits[prn])) print(min(laikad.astro_dog.orbits[prn], key=lambda e: e.epoch).epoch.as_datetime()) + if __name__ == "__main__": unittest.main() From d6c07a6b158595c1ab10256dc6eee4fbdf902379 Mon Sep 17 00:00:00 2001 From: ZwX1616 Date: Wed, 8 Jun 2022 20:13:46 -0700 Subject: [PATCH 15/93] fullframe DM model (#24762) * get log * simplify two nonsense * not needed * libyuv is a joke * clean up * try small * fast but not bad * working * clean up driverview * simplified * thats mirrored * smol * tweak * ref is screen * w/ ee * update camera model * no if TICI * start * update pose thresh * less cpu more dsp * new libyuv * new snpe * add files * test * should be fast * update out len * trigger test * use master snpe * add cereal * update cereal * refactor parsing * missing ; * get * wrong type * test model * use driver data * 10829278-72fe-4283-a118-2cef959ce174/1550 * no pf * adapt driverview * ; * rhd learner * update libyuv buildi x64 * ad4337ea * remove blink slack * test * no * use toggle * b16 * fix for nv12 * 5b02cff5 both * update test * update cereal * update cereal * update cereal * v2 packets * revert libyuv * no / * update snpemodel * ; * memcpy * fix test * use toggle in driverview * update power * update replay * Revert "update replay" This reverts commit 1d0979ca59dbc89bc5890656e9501e83f0556d50. * update model ref * halve cpu * fake 8bit onnx runner * same thresh as report * cereal master Co-authored-by: Comma Device --- cereal | 2 +- common/modeldata.h | 6 - selfdrive/camerad/cameras/camera_common.cc | 2 +- selfdrive/camerad/cameras/camera_qcom2.cc | 2 +- selfdrive/hardware/tici/test_power_draw.py | 2 +- selfdrive/modeld/dmonitoringmodeld.cc | 6 +- selfdrive/modeld/models/dmonitoring.cc | 234 ++++++------------ selfdrive/modeld/models/dmonitoring.h | 33 ++- .../modeld/models/dmonitoring_model.current | 4 +- .../modeld/models/dmonitoring_model.onnx | 4 +- .../modeld/models/dmonitoring_model_q.dlc | 4 +- selfdrive/modeld/runners/onnx_runner.py | 21 +- selfdrive/modeld/runners/onnxmodel.cc | 6 +- selfdrive/modeld/runners/onnxmodel.h | 3 +- selfdrive/modeld/runners/snpemodel.cc | 12 +- selfdrive/modeld/runners/snpemodel.h | 3 +- selfdrive/monitoring/dmonitoringd.py | 7 +- selfdrive/monitoring/driver_monitor.py | 88 +++---- selfdrive/monitoring/test_monitoring.py | 24 +- selfdrive/test/process_replay/model_replay.py | 8 +- .../process_replay/model_replay_ref_commit | 2 +- .../test/process_replay/process_replay.py | 2 +- selfdrive/test/test_onroad.py | 6 +- selfdrive/ui/qt/offroad/driverview.cc | 43 ++-- selfdrive/ui/qt/widgets/cameraview.cc | 9 +- 25 files changed, 225 insertions(+), 308 deletions(-) diff --git a/cereal b/cereal index 7cbb4f1c8c..96cbed052a 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 7cbb4f1c8cb7cfc798a058c611801442b40feb52 +Subproject commit 96cbed052ab1d69ea15da94dc2b882d59a786347 diff --git a/common/modeldata.h b/common/modeldata.h index 06d9398031..410a69ea65 100644 --- a/common/modeldata.h +++ b/common/modeldata.h @@ -24,12 +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); -namespace tici_dm_crop { - const int x_offset = -72; - const int y_offset = -144; - const int width = 954; -}; - const mat3 fcam_intrinsic_matrix = (mat3){{2648.0, 0.0, 1928.0 / 2, 0.0, 2648.0, 1208.0 / 2, 0.0, 0.0, 1.0}}; diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc index 049324f085..21e225d538 100644 --- a/selfdrive/camerad/cameras/camera_common.cc +++ b/selfdrive/camerad/cameras/camera_common.cc @@ -239,7 +239,7 @@ static kj::Array yuv420_to_jpeg(const CameraBuf *b, int thumbnail_w 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]); + 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; diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc index d43beb0921..b949dfb80f 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.cc +++ b/selfdrive/camerad/cameras/camera_qcom2.cc @@ -837,7 +837,7 @@ void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_i s->road_cam.camera_init(s, v, CAMERA_ID_AR0231, 1, 20, device_id, ctx, VISION_STREAM_RGB_ROAD, VISION_STREAM_ROAD, !env_disable_road); s->wide_road_cam.camera_init(s, v, CAMERA_ID_AR0231, 0, 20, device_id, ctx, VISION_STREAM_RGB_WIDE_ROAD, VISION_STREAM_WIDE_ROAD, !env_disable_wide_road); - s->sm = new SubMaster({"driverState"}); + s->sm = new SubMaster({"driverStateV2"}); s->pm = new PubMaster({"roadCameraState", "driverCameraState", "wideRoadCameraState", "thumbnail"}); } diff --git a/selfdrive/hardware/tici/test_power_draw.py b/selfdrive/hardware/tici/test_power_draw.py index ab2d691a09..5eb84514e5 100755 --- a/selfdrive/hardware/tici/test_power_draw.py +++ b/selfdrive/hardware/tici/test_power_draw.py @@ -21,7 +21,7 @@ class Proc: PROCS = [ Proc('camerad', 2.15), Proc('modeld', 1.0), - Proc('dmonitoringmodeld', 0.25), + Proc('dmonitoringmodeld', 0.35), Proc('encoderd', 0.23), ] diff --git a/selfdrive/modeld/dmonitoringmodeld.cc b/selfdrive/modeld/dmonitoringmodeld.cc index 68c49572e6..cde13a9bee 100644 --- a/selfdrive/modeld/dmonitoringmodeld.cc +++ b/selfdrive/modeld/dmonitoringmodeld.cc @@ -12,7 +12,7 @@ ExitHandler do_exit; void run_model(DMonitoringModelState &model, VisionIpcClient &vipc_client) { - PubMaster pm({"driverState"}); + PubMaster pm({"driverStateV2"}); SubMaster sm({"liveCalibration"}); float calib[CALIB_LEN] = {0}; double last = 0; @@ -31,11 +31,11 @@ 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, buf->stride, buf->uv_offset, calib); + DMonitoringModelResult model_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 - dmonitoring_publish(pm, extra.frame_id, res, (t2 - t1) / 1000.0, model.output); + dmonitoring_publish(pm, extra.frame_id, model_res, (t2 - t1) / 1000.0, model.output); //printf("dmonitoring process: %.2fms, from last %.2fms\n", t2 - t1, t1 - last); last = t1; diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc index e134ad3a5a..20294c3f3c 100644 --- a/selfdrive/modeld/models/dmonitoring.cc +++ b/selfdrive/modeld/models/dmonitoring.cc @@ -10,8 +10,8 @@ #include "selfdrive/modeld/models/dmonitoring.h" -constexpr int MODEL_WIDTH = 320; -constexpr int MODEL_HEIGHT = 640; +constexpr int MODEL_WIDTH = 1440; +constexpr int MODEL_HEIGHT = 960; template static inline T *get_buffer(std::vector &buf, const size_t size) { @@ -19,199 +19,115 @@ static inline T *get_buffer(std::vector &buf, const size_t size) { return buf.data(); } -static inline void init_yuv_buf(std::vector &buf, const int width, int height) { - uint8_t *y = get_buffer(buf, width * height * 3 / 2); - uint8_t *u = y + width * height; - uint8_t *v = u + (width / 2) * (height / 2); - - // needed on comma two to make the padded border black - // equivalent to RGB(0,0,0) in YUV space - memset(y, 16, width * height); - memset(u, 128, (width / 2) * (height / 2)); - memset(v, 128, (width / 2) * (height / 2)); -} - void dmonitoring_init(DMonitoringModelState* s) { s->is_rhd = Params().getBool("IsRHD"); - for (int x = 0; x < std::size(s->tensor); ++x) { - s->tensor[x] = (x - 128.f) * 0.0078125f; - } - init_yuv_buf(s->resized_buf, MODEL_WIDTH, MODEL_HEIGHT); #ifdef USE_ONNX_MODEL - s->m = new ONNXModel("models/dmonitoring_model.onnx", &s->output[0], OUTPUT_SIZE, USE_DSP_RUNTIME); + s->m = new ONNXModel("models/dmonitoring_model.onnx", &s->output[0], OUTPUT_SIZE, USE_DSP_RUNTIME, false, true); #else - s->m = new SNPEModel("models/dmonitoring_model_q.dlc", &s->output[0], OUTPUT_SIZE, USE_DSP_RUNTIME); + s->m = new SNPEModel("models/dmonitoring_model_q.dlc", &s->output[0], OUTPUT_SIZE, USE_DSP_RUNTIME, false, true); #endif s->m->addCalib(s->calib, CALIB_LEN); } -static inline auto get_yuv_buf(std::vector &buf, const int width, int height) { - uint8_t *y = get_buffer(buf, width * height * 3 / 2); - uint8_t *u = y + width * height; - uint8_t *v = u + (width /2) * (height / 2); - return std::make_tuple(y, u, v); +void parse_driver_data(DriverStateResult &ds_res, const DMonitoringModelState* s, int out_idx_offset) { + for (int i = 0; i < 3; ++i) { + ds_res.face_orientation[i] = s->output[out_idx_offset+i] * REG_SCALE; + ds_res.face_orientation_std[i] = exp(s->output[out_idx_offset+6+i]); + } + for (int i = 0; i < 2; ++i) { + ds_res.face_position[i] = s->output[out_idx_offset+3+i] * REG_SCALE; + ds_res.face_position_std[i] = exp(s->output[out_idx_offset+9+i]); + } + for (int i = 0; i < 4; ++i) { + ds_res.ready_prob[i] = sigmoid(s->output[out_idx_offset+35+i]); + } + for (int i = 0; i < 2; ++i) { + ds_res.not_ready_prob[i] = sigmoid(s->output[out_idx_offset+39+i]); + } + ds_res.face_prob = sigmoid(s->output[out_idx_offset+12]); + ds_res.left_eye_prob = sigmoid(s->output[out_idx_offset+21]); + ds_res.right_eye_prob = sigmoid(s->output[out_idx_offset+30]); + ds_res.left_blink_prob = sigmoid(s->output[out_idx_offset+31]); + ds_res.right_blink_prob = sigmoid(s->output[out_idx_offset+32]); + ds_res.sunglasses_prob = sigmoid(s->output[out_idx_offset+33]); + ds_res.occluded_prob = sigmoid(s->output[out_idx_offset+34]); } -struct Rect {int x, y, w, h;}; -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_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) * 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]; - } - } +void fill_driver_data(cereal::DriverStateV2::DriverData::Builder ddata, const DriverStateResult &ds_res) { + ddata.setFaceOrientation(ds_res.face_orientation); + ddata.setFaceOrientationStd(ds_res.face_orientation_std); + ddata.setFacePosition(ds_res.face_position); + ddata.setFacePositionStd(ds_res.face_position_std); + ddata.setFaceProb(ds_res.face_prob); + ddata.setLeftEyeProb(ds_res.left_eye_prob); + ddata.setRightEyeProb(ds_res.right_eye_prob); + ddata.setLeftBlinkProb(ds_res.left_blink_prob); + ddata.setRightBlinkProb(ds_res.right_blink_prob); + ddata.setSunglassesProb(ds_res.sunglasses_prob); + ddata.setOccludedProb(ds_res.occluded_prob); + ddata.setReadyProb(ds_res.ready_prob); + ddata.setNotReadyProb(ds_res.not_ready_prob); } -DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, int stride, int uv_offset, float *calib) { - 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; - } +DMonitoringModelResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, int stride, int uv_offset, float *calib) { + int v_off = height - MODEL_HEIGHT; + int h_off = (width - MODEL_WIDTH) / 2; + int yuv_buf_len = MODEL_WIDTH * MODEL_HEIGHT; - int resized_width = MODEL_WIDTH; - int resized_height = MODEL_HEIGHT; - - auto [cropped_y, cropped_u, cropped_v] = get_yuv_buf(s->cropped_buf, crop_rect.w, crop_rect.h); - if (!s->is_rhd) { - 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_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, - cropped_y, crop_rect.w, - cropped_u, crop_rect.w / 2, - cropped_v, crop_rect.w / 2, - crop_rect.w, crop_rect.h); - } + uint8_t *raw_buf = (uint8_t *) stream_buf; + // vertical crop free + uint8_t *raw_y_start = raw_buf + stride * v_off; - 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; - 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); - // one shot conversion, O(n) anyway - // yuvframe2tensor, normalize - for (int r = 0; r < MODEL_HEIGHT/2; r++) { - for (int c = 0; c < MODEL_WIDTH/2; c++) { - // Y_ul - net_input_buf[(r*MODEL_WIDTH/2) + c + (0*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = s->tensor[resized_y[(2*r)*resized_width + 2*c]]; - // Y_dl - net_input_buf[(r*MODEL_WIDTH/2) + c + (1*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = s->tensor[resized_y[(2*r+1)*resized_width + 2*c]]; - // Y_ur - net_input_buf[(r*MODEL_WIDTH/2) + c + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = s->tensor[resized_y[(2*r)*resized_width + 2*c+1]]; - // Y_dr - net_input_buf[(r*MODEL_WIDTH/2) + c + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = s->tensor[resized_y[(2*r+1)*resized_width + 2*c+1]]; - // U - net_input_buf[(r*MODEL_WIDTH/2) + c + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = s->tensor[resized_u[r*resized_width/2 + c]]; - // V - net_input_buf[(r*MODEL_WIDTH/2) + c + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = s->tensor[resized_v[r*resized_width/2 + c]]; - } - } - - //printf("preprocess completed. %d \n", yuv_buf_len); - //FILE *dump_yuv_file = fopen("/tmp/rawdump.yuv", "wb"); - //fwrite(resized_buf, yuv_buf_len, sizeof(uint8_t), dump_yuv_file); - //fclose(dump_yuv_file); + uint8_t *net_input_buf = get_buffer(s->net_input_buf, yuv_buf_len); - // *** testing *** - // idat = np.frombuffer(open("/tmp/inputdump.yuv", "rb").read(), np.float32).reshape(6, 160, 320) - // imshow(cv2.cvtColor(tensor_to_frames(idat[None]/0.0078125+128)[0], cv2.COLOR_YUV2RGB_I420)) + // here makes a uint8 copy + for (int r = 0; r < MODEL_HEIGHT; ++r) { + memcpy(net_input_buf + r * MODEL_WIDTH, raw_y_start + r * stride + h_off, MODEL_WIDTH); + } - //FILE *dump_yuv_file2 = fopen("/tmp/inputdump.yuv", "wb"); - //fwrite(net_input_buf, MODEL_HEIGHT*MODEL_WIDTH*3/2, sizeof(float), dump_yuv_file2); - //fclose(dump_yuv_file2); + // printf("preprocess completed. %d \n", yuv_buf_len); + // FILE *dump_yuv_file = fopen("/tmp/rawdump.yuv", "wb"); + // fwrite(net_input_buf, yuv_buf_len, sizeof(uint8_t), dump_yuv_file); + // fclose(dump_yuv_file); double t1 = millis_since_boot(); - s->m->addImage(net_input_buf, yuv_buf_len); + s->m->addImage((float*)net_input_buf, yuv_buf_len / 4); for (int i = 0; i < CALIB_LEN; i++) { s->calib[i] = calib[i]; } s->m->execute(); double t2 = millis_since_boot(); - DMonitoringResult ret = {0}; - for (int i = 0; i < 3; ++i) { - ret.face_orientation[i] = s->output[i] * REG_SCALE; - ret.face_orientation_meta[i] = exp(s->output[6 + i]); - } - for (int i = 0; i < 2; ++i) { - ret.face_position[i] = s->output[3 + i] * REG_SCALE; - ret.face_position_meta[i] = exp(s->output[9 + i]); - } - for (int i = 0; i < 4; ++i) { - ret.ready_prob[i] = sigmoid(s->output[39 + i]); - } - for (int i = 0; i < 2; ++i) { - ret.not_ready_prob[i] = sigmoid(s->output[43 + i]); - } - ret.face_prob = sigmoid(s->output[12]); - ret.left_eye_prob = sigmoid(s->output[21]); - ret.right_eye_prob = sigmoid(s->output[30]); - ret.left_blink_prob = sigmoid(s->output[31]); - ret.right_blink_prob = sigmoid(s->output[32]); - ret.sg_prob = sigmoid(s->output[33]); - ret.poor_vision = sigmoid(s->output[34]); - ret.partial_face = sigmoid(s->output[35]); - ret.distracted_pose = sigmoid(s->output[36]); - ret.distracted_eyes = sigmoid(s->output[37]); - ret.occluded_prob = sigmoid(s->output[38]); - ret.dsp_execution_time = (t2 - t1) / 1000.; - return ret; + DMonitoringModelResult model_res = {0}; + parse_driver_data(model_res.driver_state_lhd, s, 0); + parse_driver_data(model_res.driver_state_rhd, s, 41); + model_res.poor_vision_prob = sigmoid(s->output[82]); + model_res.wheel_on_right_prob = sigmoid(s->output[83]); + model_res.dsp_execution_time = (t2 - t1) / 1000.; + + return model_res; } -void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, float execution_time, kj::ArrayPtr raw_pred) { +void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringModelResult &model_res, float execution_time, kj::ArrayPtr raw_pred) { // make msg MessageBuilder msg; - auto framed = msg.initEvent().initDriverState(); + auto framed = msg.initEvent().initDriverStateV2(); framed.setFrameId(frame_id); framed.setModelExecutionTime(execution_time); - framed.setDspExecutionTime(res.dsp_execution_time); - - framed.setFaceOrientation(res.face_orientation); - framed.setFaceOrientationStd(res.face_orientation_meta); - framed.setFacePosition(res.face_position); - framed.setFacePositionStd(res.face_position_meta); - framed.setFaceProb(res.face_prob); - framed.setLeftEyeProb(res.left_eye_prob); - framed.setRightEyeProb(res.right_eye_prob); - framed.setLeftBlinkProb(res.left_blink_prob); - framed.setRightBlinkProb(res.right_blink_prob); - framed.setSunglassesProb(res.sg_prob); - framed.setPoorVision(res.poor_vision); - framed.setPartialFace(res.partial_face); - framed.setDistractedPose(res.distracted_pose); - framed.setDistractedEyes(res.distracted_eyes); - framed.setOccludedProb(res.occluded_prob); - framed.setReadyProb(res.ready_prob); - framed.setNotReadyProb(res.not_ready_prob); + framed.setDspExecutionTime(model_res.dsp_execution_time); + + framed.setPoorVisionProb(model_res.poor_vision_prob); + framed.setWheelOnRightProb(model_res.wheel_on_right_prob); + fill_driver_data(framed.initLeftDriverData(), model_res.driver_state_lhd); + fill_driver_data(framed.initRightDriverData(), model_res.driver_state_rhd); + if (send_raw_pred) { framed.setRawPredictions(raw_pred.asBytes()); } - pm.send("driverState", msg); + pm.send("driverStateV2", msg); } void dmonitoring_free(DMonitoringModelState* s) { diff --git a/selfdrive/modeld/models/dmonitoring.h b/selfdrive/modeld/models/dmonitoring.h index a1be91e3bb..874722cd93 100644 --- a/selfdrive/modeld/models/dmonitoring.h +++ b/selfdrive/modeld/models/dmonitoring.h @@ -9,44 +9,43 @@ #define CALIB_LEN 3 -#define OUTPUT_SIZE 45 +#define OUTPUT_SIZE 84 #define REG_SCALE 0.25f -typedef struct DMonitoringResult { +typedef struct DriverStateResult { float face_orientation[3]; - float face_orientation_meta[3]; + float face_orientation_std[3]; float face_position[2]; - float face_position_meta[2]; + float face_position_std[2]; float face_prob; float left_eye_prob; float right_eye_prob; float left_blink_prob; float right_blink_prob; - float sg_prob; - float poor_vision; - float partial_face; - float distracted_pose; - float distracted_eyes; + float sunglasses_prob; float occluded_prob; float ready_prob[4]; float not_ready_prob[2]; +} DriverStateResult; + +typedef struct DMonitoringModelResult { + DriverStateResult driver_state_lhd; + DriverStateResult driver_state_rhd; + float poor_vision_prob; + float wheel_on_right_prob; float dsp_execution_time; -} DMonitoringResult; +} DMonitoringModelResult; typedef struct DMonitoringModelState { RunModel *m; bool is_rhd; float output[OUTPUT_SIZE]; - std::vector resized_buf; - std::vector cropped_buf; - std::vector premirror_cropped_buf; - std::vector net_input_buf; + std::vector net_input_buf; float calib[CALIB_LEN]; - float tensor[UINT8_MAX + 1]; } DMonitoringModelState; void dmonitoring_init(DMonitoringModelState* s); -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); +DMonitoringModelResult 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 DMonitoringModelResult &model_res, float execution_time, kj::ArrayPtr raw_pred); void dmonitoring_free(DMonitoringModelState* s); diff --git a/selfdrive/modeld/models/dmonitoring_model.current b/selfdrive/modeld/models/dmonitoring_model.current index 74bcfe17a4..ba2865429e 100644 --- a/selfdrive/modeld/models/dmonitoring_model.current +++ b/selfdrive/modeld/models/dmonitoring_model.current @@ -1,2 +1,2 @@ -a8236e30-5bee-4689-8ea0-fc102e2770e5 -d508c79bae1c1c451f3af3e2bc231ce33678cb43 \ No newline at end of file +5b02cff5-2b29-431d-b186-372e9c6fd0c7 +bf33cc569076984626ac7e027f927aa593261fa7 \ No newline at end of file diff --git a/selfdrive/modeld/models/dmonitoring_model.onnx b/selfdrive/modeld/models/dmonitoring_model.onnx index 51b0d1ed76..1fca1317d5 100644 --- a/selfdrive/modeld/models/dmonitoring_model.onnx +++ b/selfdrive/modeld/models/dmonitoring_model.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:00731ebd06fcff7e5837607b91bc56cad3bed5d7ee89052c911c981e8f665308 -size 3679940 +oid sha256:aca1dd411b5f488bea605dc360656e631fc4301968a589ea072e2220c8092600 +size 9157561 diff --git a/selfdrive/modeld/models/dmonitoring_model_q.dlc b/selfdrive/modeld/models/dmonitoring_model_q.dlc index 2e54f7ee4b..c1cba15176 100644 --- a/selfdrive/modeld/models/dmonitoring_model_q.dlc +++ b/selfdrive/modeld/models/dmonitoring_model_q.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:667df5e925570a0f6a33dfb890e186a1f13f101885b46db47ec45305737dffb6 -size 1145921 +oid sha256:d146b1d1fd9d40d57d058e51d285f83676866e26d9e5aff9fa27623ce343b58a +size 2636941 diff --git a/selfdrive/modeld/runners/onnx_runner.py b/selfdrive/modeld/runners/onnx_runner.py index e282a66b66..ac7cc68814 100755 --- a/selfdrive/modeld/runners/onnx_runner.py +++ b/selfdrive/modeld/runners/onnx_runner.py @@ -9,20 +9,24 @@ os.environ["OMP_WAIT_POLICY"] = "PASSIVE" import onnxruntime as ort # pylint: disable=import-error -def read(sz): +def read(sz, tf8=False): dd = [] gt = 0 - while gt < sz * 4: - st = os.read(0, sz * 4 - gt) + szof = 1 if tf8 else 4 + while gt < sz * szof: + st = os.read(0, sz * szof - gt) assert(len(st) > 0) dd.append(st) gt += len(st) - return np.frombuffer(b''.join(dd), dtype=np.float32) + r = np.frombuffer(b''.join(dd), dtype=np.uint8 if tf8 else np.float32).astype(np.float32) + if tf8: + r = r / 255. + return r def write(d): os.write(1, d.tobytes()) -def run_loop(m): +def run_loop(m, tf8_input=False): ishapes = [[1]+ii.shape[1:] for ii in m.get_inputs()] keys = [x.name for x in m.get_inputs()] @@ -33,10 +37,10 @@ def run_loop(m): print("ready to run onnx model", keys, ishapes, file=sys.stderr) while 1: inputs = [] - for shp in ishapes: + for k, shp in zip(keys, ishapes): ts = np.product(shp) #print("reshaping %s with offset %d" % (str(shp), offset), file=sys.stderr) - inputs.append(read(ts).reshape(shp)) + inputs.append(read(ts, (k=='input_img' and tf8_input)).reshape(shp)) ret = m.run(None, dict(zip(keys, inputs))) #print(ret, file=sys.stderr) for r in ret: @@ -44,6 +48,7 @@ def run_loop(m): if __name__ == "__main__": + print(sys.argv, file=sys.stderr) print("Onnx available providers: ", ort.get_available_providers(), file=sys.stderr) options = ort.SessionOptions() options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_DISABLE_ALL @@ -63,6 +68,6 @@ if __name__ == "__main__": print("Onnx selected provider: ", [provider], file=sys.stderr) ort_session = ort.InferenceSession(sys.argv[1], options, providers=[provider]) print("Onnx using ", ort_session.get_providers(), file=sys.stderr) - run_loop(ort_session) + run_loop(ort_session, tf8_input=("--use_tf8" in sys.argv)) except KeyboardInterrupt: pass diff --git a/selfdrive/modeld/runners/onnxmodel.cc b/selfdrive/modeld/runners/onnxmodel.cc index 9b4d6fd015..1f9f551abc 100644 --- a/selfdrive/modeld/runners/onnxmodel.cc +++ b/selfdrive/modeld/runners/onnxmodel.cc @@ -14,12 +14,13 @@ #include "common/swaglog.h" #include "common/util.h" -ONNXModel::ONNXModel(const char *path, float *_output, size_t _output_size, int runtime, bool _use_extra) { +ONNXModel::ONNXModel(const char *path, float *_output, size_t _output_size, int runtime, bool _use_extra, bool _use_tf8) { LOGD("loading model %s", path); output = _output; output_size = _output_size; use_extra = _use_extra; + use_tf8 = _use_tf8; int err = pipe(pipein); assert(err == 0); @@ -28,11 +29,12 @@ ONNXModel::ONNXModel(const char *path, float *_output, size_t _output_size, int std::string exe_dir = util::dir_name(util::readlink("/proc/self/exe")); std::string onnx_runner = exe_dir + "/runners/onnx_runner.py"; + std::string tf8_arg = use_tf8 ? "--use_tf8" : ""; proc_pid = fork(); if (proc_pid == 0) { LOGD("spawning onnx process %s", onnx_runner.c_str()); - char *argv[] = {(char*)onnx_runner.c_str(), (char*)path, nullptr}; + char *argv[] = {(char*)onnx_runner.c_str(), (char*)path, (char*)tf8_arg.c_str(), nullptr}; dup2(pipein[0], 0); dup2(pipeout[1], 1); close(pipein[0]); diff --git a/selfdrive/modeld/runners/onnxmodel.h b/selfdrive/modeld/runners/onnxmodel.h index 567d81d29e..4ac599e2af 100644 --- a/selfdrive/modeld/runners/onnxmodel.h +++ b/selfdrive/modeld/runners/onnxmodel.h @@ -6,7 +6,7 @@ class ONNXModel : public RunModel { public: - ONNXModel(const char *path, float *output, size_t output_size, int runtime, bool use_extra = false); + ONNXModel(const char *path, float *output, size_t output_size, int runtime, bool use_extra = false, bool _use_tf8 = false); ~ONNXModel(); void addRecurrent(float *state, int state_size); void addDesire(float *state, int state_size); @@ -31,6 +31,7 @@ private: int calib_size; float *image_input_buf = NULL; int image_buf_size; + bool use_tf8; float *extra_input_buf = NULL; int extra_buf_size; bool use_extra; diff --git a/selfdrive/modeld/runners/snpemodel.cc b/selfdrive/modeld/runners/snpemodel.cc index 1861494d59..4d6917e894 100644 --- a/selfdrive/modeld/runners/snpemodel.cc +++ b/selfdrive/modeld/runners/snpemodel.cc @@ -14,10 +14,11 @@ void PrintErrorStringAndExit() { std::exit(EXIT_FAILURE); } -SNPEModel::SNPEModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra) { +SNPEModel::SNPEModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra, bool luse_tf8) { output = loutput; output_size = loutput_size; use_extra = luse_extra; + use_tf8 = luse_tf8; #ifdef QCOM2 if (runtime==USE_GPU_RUNTIME) { Runtime = zdl::DlSystem::Runtime_t::GPU; @@ -70,14 +71,16 @@ SNPEModel::SNPEModel(const char *path, float *loutput, size_t loutput_size, int printf("model: %s -> %s\n", input_tensor_name, output_tensor_name); zdl::DlSystem::UserBufferEncodingFloat userBufferEncodingFloat; + zdl::DlSystem::UserBufferEncodingTf8 userBufferEncodingTf8(0, 1./255); // network takes 0-1 zdl::DlSystem::IUserBufferFactory& ubFactory = zdl::SNPE::SNPEFactory::getUserBufferFactory(); + size_t size_of_input = use_tf8 ? sizeof(uint8_t) : sizeof(float); // create input buffer { const auto &inputDims_opt = snpe->getInputDimensions(input_tensor_name); const zdl::DlSystem::TensorShape& bufferShape = *inputDims_opt; std::vector strides(bufferShape.rank()); - strides[strides.size() - 1] = sizeof(float); + strides[strides.size() - 1] = size_of_input; size_t product = 1; for (size_t i = 0; i < bufferShape.rank(); i++) product *= bufferShape[i]; size_t stride = strides[strides.size() - 1]; @@ -86,7 +89,10 @@ SNPEModel::SNPEModel(const char *path, float *loutput, size_t loutput_size, int strides[i-1] = stride; } printf("input product is %lu\n", product); - inputBuffer = ubFactory.createUserBuffer(NULL, product*sizeof(float), strides, &userBufferEncodingFloat); + inputBuffer = ubFactory.createUserBuffer(NULL, + product*size_of_input, + strides, + use_tf8 ? (zdl::DlSystem::UserBufferEncoding*)&userBufferEncodingTf8 : (zdl::DlSystem::UserBufferEncoding*)&userBufferEncodingFloat); inputMap.add(input_tensor_name, inputBuffer.get()); } diff --git a/selfdrive/modeld/runners/snpemodel.h b/selfdrive/modeld/runners/snpemodel.h index ba51fdced0..ed9d58d1e1 100644 --- a/selfdrive/modeld/runners/snpemodel.h +++ b/selfdrive/modeld/runners/snpemodel.h @@ -23,7 +23,7 @@ class SNPEModel : public RunModel { public: - SNPEModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra = false); + SNPEModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra = false, bool use_tf8 = false); void addRecurrent(float *state, int state_size); void addTrafficConvention(float *state, int state_size); void addCalib(float *state, int state_size); @@ -52,6 +52,7 @@ private: std::unique_ptr inputBuffer; float *input; size_t input_size; + bool use_tf8; // snpe output stuff zdl::DlSystem::UserBufferMap outputMap; diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index 9a3196030b..c31e9da43b 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -18,7 +18,7 @@ def dmonitoringd_thread(sm=None, pm=None): pm = messaging.PubMaster(['driverMonitoringState']) if sm is None: - sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll=['driverState']) + sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll=['driverStateV2']) driver_status = DriverStatus(rhd=Params().get_bool("IsRHD")) @@ -34,7 +34,7 @@ def dmonitoringd_thread(sm=None, pm=None): while True: sm.update() - if not sm.updated['driverState']: + if not sm.updated['driverStateV2']: continue # Get interaction @@ -51,7 +51,7 @@ def dmonitoringd_thread(sm=None, pm=None): # Get data from dmonitoringmodeld events = Events() - driver_status.update_states(sm['driverState'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled) + driver_status.update_states(sm['driverStateV2'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled) # Block engaging after max number of distrations if driver_status.terminal_alert_cnt >= driver_status.settings._MAX_TERMINAL_ALERTS or \ @@ -79,6 +79,7 @@ def dmonitoringd_thread(sm=None, pm=None): "isLowStd": driver_status.pose.low_std, "hiStdCount": driver_status.hi_stds, "isActiveMode": driver_status.active_monitoring_mode, + "isRHD": driver_status.wheel_on_right, } pm.send('driverMonitoringState', dat) diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py index 662e0d76ce..4affa1e796 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/selfdrive/monitoring/driver_monitor.py @@ -5,6 +5,7 @@ from common.numpy_fast import interp from common.realtime import DT_DMON from common.filter_simple import FirstOrderFilter from common.stat_live import RunningStatFilter +from common.transformations.camera import tici_d_frame_size EventName = car.CarEvent.EventName @@ -26,32 +27,29 @@ class DRIVER_MONITOR_SETTINGS(): self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6. self._FACE_THRESHOLD = 0.5 - 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._BLINK_THRESHOLD = 0.861 self._EE_THRESH11 = 0.75 self._EE_THRESH12 = 3.25 self._EE_THRESH21 = 0.01 self._EE_THRESH22 = 0.35 - self._POSE_PITCH_THRESHOLD = 0.3237 - self._POSE_PITCH_THRESHOLD_SLACK = 0.3657 + self._POSE_PITCH_THRESHOLD = 0.3133 + self._POSE_PITCH_THRESHOLD_SLACK = 0.3237 self._POSE_PITCH_THRESHOLD_STRICT = self._POSE_PITCH_THRESHOLD - self._POSE_YAW_THRESHOLD = 0.3109 - self._POSE_YAW_THRESHOLD_SLACK = 0.4294 + self._POSE_YAW_THRESHOLD = 0.4020 + self._POSE_YAW_THRESHOLD_SLACK = 0.5042 self._POSE_YAW_THRESHOLD_STRICT = self._POSE_YAW_THRESHOLD - self._PITCH_NATURAL_OFFSET = 0.057 # initial value before offset is learned - self._YAW_NATURAL_OFFSET = 0.11 # initial value before offset is learned + self._PITCH_NATURAL_OFFSET = 0.029 # initial value before offset is learned + self._YAW_NATURAL_OFFSET = 0.097 # initial value before offset is learned self._PITCH_MAX_OFFSET = 0.124 self._PITCH_MIN_OFFSET = -0.0881 self._YAW_MAX_OFFSET = 0.289 self._YAW_MIN_OFFSET = -0.0246 - self._POSESTD_THRESHOLD = 0.315 + self._POSESTD_THRESHOLD = 0.3 self._HI_STD_FALLBACK_TIME = int(10 / self._DT_DMON) # fall back to wheel touch if model is uncertain for 10s self._DISTRACTED_FILTER_TS = 0.25 # 0.6Hz @@ -59,6 +57,9 @@ class DRIVER_MONITOR_SETTINGS(): self._POSE_OFFSET_MIN_COUNT = int(60 / self._DT_DMON) # valid data counts before calibration completes, 1min cumulative self._POSE_OFFSET_MAX_COUNT = int(360 / self._DT_DMON) # stop deweighting new data after 6 min, aka "short term memory" + self._WHEELPOS_THRESHOLD = 0.5 + self._WHEELPOS_FILTER_MIN_COUNT = int(5 / self._DT_DMON) + self._RECOVERY_FACTOR_MAX = 5. # relative to minus step change self._RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change @@ -66,9 +67,9 @@ class DRIVER_MONITOR_SETTINGS(): self._MAX_TERMINAL_DURATION = int(30 / self._DT_DMON) # not allowed to engage after 30s of terminal alerts -# model output refers to center of cropped image, so need to apply the x displacement offset -RESIZED_FOCAL = 320.0 -H, W, FULL_W = 320, 160, 426 +# model output refers to center of undistorted+leveled image +EFL = 598.0 # focal length in K +W, H = tici_d_frame_size # corrected image has same size as raw class DistractedType: NOT_DISTRACTED = 0 @@ -76,22 +77,22 @@ class DistractedType: DISTRACTED_BLINK = 2 DISTRACTED_E2E = 4 -def face_orientation_from_net(angles_desc, pos_desc, rpy_calib, is_rhd): +def face_orientation_from_net(angles_desc, pos_desc, rpy_calib): # the output of these angles are in device frame # so from driver's perspective, pitch is up and yaw is right pitch_net, yaw_net, roll_net = angles_desc - face_pixel_position = ((pos_desc[0] + .5)*W - W + FULL_W, (pos_desc[1]+.5)*H) - yaw_focal_angle = atan2(face_pixel_position[0] - FULL_W//2, RESIZED_FOCAL) - pitch_focal_angle = atan2(face_pixel_position[1] - H//2, RESIZED_FOCAL) + face_pixel_position = ((pos_desc[0]+0.5)*W, (pos_desc[1]+0.5)*H) + yaw_focal_angle = atan2(face_pixel_position[0] - W//2, EFL) + pitch_focal_angle = atan2(face_pixel_position[1] - H//2, EFL) pitch = pitch_net + pitch_focal_angle yaw = -yaw_net + yaw_focal_angle # no calib for roll pitch -= rpy_calib[1] - yaw -= rpy_calib[2] * (1 - 2 * int(is_rhd)) # lhd -> -=, rhd -> += + yaw -= rpy_calib[2] return roll_net, pitch, yaw class DriverPose(): @@ -112,7 +113,6 @@ class DriverBlink(): def __init__(self): self.left_blink = 0. self.right_blink = 0. - self.cfactor = 1. class DriverStatus(): def __init__(self, rhd=False, settings=DRIVER_MONITOR_SETTINGS()): @@ -120,7 +120,7 @@ class DriverStatus(): self.settings = settings # init driver status - self.is_rhd_region = rhd + # self.wheelpos_learner = RunningStatFilter() self.pose = DriverPose(self.settings._POSE_OFFSET_MAX_COUNT) self.pose_calibrated = False self.blink = DriverBlink() @@ -137,8 +137,8 @@ class DriverStatus(): self.distracted_types = [] self.driver_distracted = False self.driver_distraction_filter = FirstOrderFilter(0., self.settings._DISTRACTED_FILTER_TS, self.settings._DT_DMON) + self.wheel_on_right = rhd self.face_detected = False - self.face_partial = False self.terminal_alert_cnt = 0 self.terminal_time = 0 self.step_change = 0. @@ -197,7 +197,7 @@ class DriverStatus(): yaw_error > self.settings._POSE_YAW_THRESHOLD*self.pose.cfactor_yaw: distracted_types.append(DistractedType.DISTRACTED_POSE) - if (self.blink.left_blink + self.blink.right_blink)*0.5 > self.settings._BLINK_THRESHOLD*self.blink.cfactor: + if (self.blink.left_blink + self.blink.right_blink)*0.5 > self.settings._BLINK_THRESHOLD: distracted_types.append(DistractedType.DISTRACTED_BLINK) if self.ee1_calibrated: @@ -214,13 +214,7 @@ class DriverStatus(): return distracted_types def set_policy(self, model_data, car_speed): - ep = min(model_data.meta.engagedProb, 0.8) / 0.8 # engaged prob bp = model_data.meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s - # TODO: retune adaptive blink - self.blink.cfactor = interp(ep, [0, 0.5, 1], - [self.settings._BLINK_THRESHOLD_STRICT, - self.settings._BLINK_THRESHOLD, - self.settings._BLINK_THRESHOLD_SLACK]) / self.settings._BLINK_THRESHOLD k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2) bp_normal = max(min(bp / k1, 0.5),0) self.pose.cfactor_pitch = interp(bp_normal, [0, 0.5], @@ -231,28 +225,34 @@ class DriverStatus(): self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD def update_states(self, driver_state, cal_rpy, car_speed, op_engaged): - if not all(len(x) > 0 for x in (driver_state.faceOrientation, driver_state.facePosition, - driver_state.faceOrientationStd, driver_state.facePositionStd, - driver_state.readyProb, driver_state.notReadyProb)): + # rhd_pred = driver_state.wheelOnRightProb + # if car_speed > 0.01: + # self.wheelpos_learner.push_and_update(rhd_pred) + # if self.wheelpos_learner.filtered_stat.n > self.settings._WHEELPOS_FILTER_MIN_COUNT: + # self.wheel_on_right = self.wheelpos_learner.filtered_stat.M > self.settings._WHEELPOS_THRESHOLD + # else: + # self.wheel_on_right = rhd_pred > self.settings._WHEELPOS_THRESHOLD + driver_data = driver_state.rightDriverData if self.wheel_on_right else driver_state.leftDriverData + if not all(len(x) > 0 for x in (driver_data.faceOrientation, driver_data.facePosition, + driver_data.faceOrientationStd, driver_data.facePositionStd, + driver_data.readyProb, driver_data.notReadyProb)): return - self.face_partial = driver_state.partialFace > self.settings._PARTIAL_FACE_THRESHOLD - self.face_detected = driver_state.faceProb > self.settings._FACE_THRESHOLD or self.face_partial - self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_state.faceOrientation, driver_state.facePosition, cal_rpy, self.is_rhd_region) - self.pose.pitch_std = driver_state.faceOrientationStd[0] - self.pose.yaw_std = driver_state.faceOrientationStd[1] - # self.pose.roll_std = driver_state.faceOrientationStd[2] + self.face_detected = driver_data.faceProb > self.settings._FACE_THRESHOLD + self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_data.faceOrientation, driver_data.facePosition, cal_rpy) + self.pose.pitch_std = driver_data.faceOrientationStd[0] + self.pose.yaw_std = driver_data.faceOrientationStd[1] model_std_max = max(self.pose.pitch_std, self.pose.yaw_std) - self.pose.low_std = model_std_max < self.settings._POSESTD_THRESHOLD and not self.face_partial - self.blink.left_blink = driver_state.leftBlinkProb * (driver_state.leftEyeProb > self.settings._EYE_THRESHOLD) * (driver_state.sunglassesProb < self.settings._SG_THRESHOLD) - self.blink.right_blink = driver_state.rightBlinkProb * (driver_state.rightEyeProb > self.settings._EYE_THRESHOLD) * (driver_state.sunglassesProb < self.settings._SG_THRESHOLD) - self.eev1 = driver_state.notReadyProb[1] - self.eev2 = driver_state.readyProb[0] + self.pose.low_std = model_std_max < self.settings._POSESTD_THRESHOLD + self.blink.left_blink = driver_data.leftBlinkProb * (driver_data.leftEyeProb > self.settings._EYE_THRESHOLD) * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD) + self.blink.right_blink = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD) + self.eev1 = driver_data.notReadyProb[1] + self.eev2 = driver_data.readyProb[0] self.distracted_types = self._get_distracted_types() self.driver_distracted = (DistractedType.DISTRACTED_POSE in self.distracted_types or DistractedType.DISTRACTED_BLINK in self.distracted_types) and \ - driver_state.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std + driver_data.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std self.driver_distraction_filter.update(self.driver_distracted) # update offseter diff --git a/selfdrive/monitoring/test_monitoring.py b/selfdrive/monitoring/test_monitoring.py index a84ed242ba..43b5e7747e 100755 --- a/selfdrive/monitoring/test_monitoring.py +++ b/selfdrive/monitoring/test_monitoring.py @@ -17,19 +17,19 @@ INVISIBLE_SECONDS_TO_ORANGE = dm_settings._AWARENESS_TIME - dm_settings._AWARENE INVISIBLE_SECONDS_TO_RED = dm_settings._AWARENESS_TIME + 1 def make_msg(face_detected, distracted=False, model_uncertain=False): - ds = log.DriverState.new_message() - ds.faceOrientation = [0., 0., 0.] - ds.facePosition = [0., 0.] - ds.faceProb = 1. * face_detected - ds.leftEyeProb = 1. - ds.rightEyeProb = 1. - ds.leftBlinkProb = 1. * distracted - ds.rightBlinkProb = 1. * distracted - ds.faceOrientationStd = [1.*model_uncertain, 1.*model_uncertain, 1.*model_uncertain] - ds.facePositionStd = [1.*model_uncertain, 1.*model_uncertain] + ds = log.DriverStateV2.new_message() + ds.leftDriverData.faceOrientation = [0., 0., 0.] + ds.leftDriverData.facePosition = [0., 0.] + ds.leftDriverData.faceProb = 1. * face_detected + ds.leftDriverData.leftEyeProb = 1. + ds.leftDriverData.rightEyeProb = 1. + ds.leftDriverData.leftBlinkProb = 1. * distracted + ds.leftDriverData.rightBlinkProb = 1. * distracted + ds.leftDriverData.faceOrientationStd = [1.*model_uncertain, 1.*model_uncertain, 1.*model_uncertain] + ds.leftDriverData.facePositionStd = [1.*model_uncertain, 1.*model_uncertain] # TODO: test both separately when e2e is used - ds.readyProb = [0., 0., 0., 0.] - ds.notReadyProb = [0., 0.] + ds.leftDriverData.readyProb = [0., 0., 0., 0.] + ds.leftDriverData.notReadyProb = [0., 0.] return ds diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py index b83629c76a..032b504879 100755 --- a/selfdrive/test/process_replay/model_replay.py +++ b/selfdrive/test/process_replay/model_replay.py @@ -52,7 +52,7 @@ def model_replay(lr, frs): vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, *(tici_f_frame_size)) vipc_server.start_listener() - sm = messaging.SubMaster(['modelV2', 'driverState']) + sm = messaging.SubMaster(['modelV2', 'driverStateV2']) pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'driverCameraState', 'liveCalibration', 'lateralPlan']) try: @@ -112,7 +112,7 @@ def model_replay(lr, frs): if min(frame_idxs['roadCameraState'], frame_idxs['wideRoadCameraState']) > recv_cnt['modelV2']: recv = "modelV2" elif msg.which() == 'driverCameraState': - recv = "driverState" + recv = "driverStateV2" # wait for a response with Timeout(15, f"timed out waiting for {recv}"): @@ -170,8 +170,8 @@ if __name__ == "__main__": 'logMonoTime', 'modelV2.frameDropPerc', 'modelV2.modelExecutionTime', - 'driverState.modelExecutionTime', - 'driverState.dspExecutionTime' + 'driverStateV2.modelExecutionTime', + 'driverStateV2.dspExecutionTime' ] # TODO this tolerence is absurdly large tolerance = 5e-1 if PC else None diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index 90520f2619..626d54a8f0 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -f74ab97371be93fdc28333e5ea12bbb78c3a32d0 +1d0979ca59dbc89bc5890656e9501e83f0556d50 diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 1eda9bc7f5..89885ef397 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -293,7 +293,7 @@ CONFIGS = [ ProcessConfig( proc_name="dmonitoringd", pub_sub={ - "driverState": ["driverMonitoringState"], + "driverStateV2": ["driverMonitoringState"], "liveCalibration": [], "carState": [], "modelV2": [], "controlsState": [], }, ignore=["logMonoTime", "valid"], diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 6cf53a046e..3beeaff3b2 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -33,7 +33,7 @@ PROCS = { "selfdrive.controls.radard": 4.5, "./_modeld": 4.48, "./boardd": 3.63, - "./_dmonitoringmodeld": 10.0, + "./_dmonitoringmodeld": 5.0, "selfdrive.thermald.thermald": 3.87, "selfdrive.locationd.calibrationd": 2.0, "./_soundd": 1.0, @@ -60,7 +60,7 @@ TIMINGS = { "roadCameraState": [2.5, 0.35], "driverCameraState": [2.5, 0.35], "modelV2": [2.5, 0.35], - "driverState": [2.5, 0.40], + "driverStateV2": [2.5, 0.40], "liveLocationKalman": [2.5, 0.35], "wideRoadCameraState": [1.5, 0.35], } @@ -221,7 +221,7 @@ class TestOnroad(unittest.TestCase): # TODO: this went up when plannerd cpu usage increased, why? cfgs = [ ("modelV2", 0.050, 0.036), - ("driverState", 0.050, 0.026), + ("driverStateV2", 0.050, 0.026), ] for (s, instant_max, avg_max) in cfgs: ts = [getattr(getattr(m, s), "modelExecutionTime") for m in self.lr if m.which() == s] diff --git a/selfdrive/ui/qt/offroad/driverview.cc b/selfdrive/ui/qt/offroad/driverview.cc index 06578b455a..868a97f604 100644 --- a/selfdrive/ui/qt/offroad/driverview.cc +++ b/selfdrive/ui/qt/offroad/driverview.cc @@ -25,7 +25,7 @@ void DriverViewWindow::mouseReleaseEvent(QMouseEvent* e) { emit done(); } -DriverViewScene::DriverViewScene(QWidget* parent) : sm({"driverState"}), QWidget(parent) { +DriverViewScene::DriverViewScene(QWidget* parent) : sm({"driverStateV2"}), QWidget(parent) { face_img = loadPixmap("../assets/img_driver_face.png", {FACE_IMG_SIZE, FACE_IMG_SIZE}); } @@ -57,43 +57,36 @@ void DriverViewScene::paintEvent(QPaintEvent* event) { return; } - const int width = 4 * height() / 3; - const QRect rect2 = {rect().center().x() - width / 2, rect().top(), width, rect().height()}; - const QRect valid_rect = {is_rhd ? rect2.right() - rect2.height() / 2 : rect2.left(), rect2.top(), rect2.height() / 2, rect2.height()}; + cereal::DriverStateV2::Reader driver_state = sm["driverStateV2"].getDriverStateV2(); + cereal::DriverStateV2::DriverData::Reader driver_data; - // blackout - const QColor bg(0, 0, 0, 140); - 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); - 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 + // is_rhd = driver_state.getWheelOnRightProb() > 0.5; + driver_data = is_rhd ? driver_state.getRightDriverData() : driver_state.getLeftDriverData(); - // face bounding box - cereal::DriverState::Reader driver_state = sm["driverState"].getDriverState(); - bool face_detected = driver_state.getFaceProb() > 0.5; + bool face_detected = driver_data.getFaceProb() > 0.5; if (face_detected) { - auto fxy_list = driver_state.getFacePosition(); - auto std_list = driver_state.getFaceOrientationStd(); + auto fxy_list = driver_data.getFacePosition(); + auto std_list = driver_data.getFaceOrientationStd(); float face_x = fxy_list[0]; float face_y = fxy_list[1]; float face_std = std::max(std_list[0], std_list[1]); float alpha = 0.7; - if (face_std > 0.08) { - alpha = std::max(0.7 - (face_std-0.08)*7, 0.0); + if (face_std > 0.15) { + alpha = std::max(0.7 - (face_std-0.15)*3.5, 0.0); } - const int box_size = 0.6 * rect2.height() / 2; - const float rhd_offset = 0.05; // lhd is shifted, so rhd is not mirrored - int fbox_x = valid_rect.center().x() + (is_rhd ? (face_x + rhd_offset) : -face_x) * valid_rect.width(); - int fbox_y = valid_rect.center().y() + face_y * valid_rect.height(); + const int box_size = 220; + // use approx instead of distort_points + int fbox_x = 1080.0 - 1714.0 * face_x; + int fbox_y = -135.0 + (504.0 + std::abs(face_x)*112.0) + (1205.0 - std::abs(face_x)*724.0) * face_y; p.setPen(QPen(QColor(255, 255, 255, alpha * 255), 10)); p.drawRoundedRect(fbox_x - box_size / 2, fbox_y - box_size / 2, box_size, box_size, 35.0, 35.0); } // icon - const int img_offset = 30; - const int img_x = is_rhd ? rect2.right() - FACE_IMG_SIZE - img_offset : rect2.left() + img_offset; - const int img_y = rect2.bottom() - FACE_IMG_SIZE - img_offset; - p.setOpacity(face_detected ? 1.0 : 0.3); + const int img_offset = 60; + const int img_x = rect().left() + img_offset; + const int img_y = rect().bottom() - FACE_IMG_SIZE - img_offset; + p.setOpacity(face_detected ? 1.0 : 0.2); p.drawPixmap(img_x, img_y, face_img); } diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index f16e8e4e0d..0a3b9762e5 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -54,16 +54,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; - const float yscale = stream_height * driver_view_ratio / tici_dm_crop::width; + const float driver_view_ratio = 2.0; + const float yscale = stream_height * driver_view_ratio / stream_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, + xscale, 0.0, 0.0, 0.0, + 0.0, yscale, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, }}; - return transform; } From 19af336f89807a3a621132c5054f68de2b49d43c Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 8 Jun 2022 21:07:49 -0700 Subject: [PATCH 16/93] bump opendbc (#24790) --- opendbc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc b/opendbc index 7701277d26..30aacafa10 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 7701277d2666119bc7fcaca9f8cfefd50cd5b071 +Subproject commit 30aacafa10e72db33ecf5e3b7a6eacffa9390b8b From 384e08e2ee299e8e0bc558024361ab92bedb1bbd Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 8 Jun 2022 22:38:07 -0700 Subject: [PATCH 17/93] Honda Bosch: fix openpilot longitudinal controls mismatch (#24788) * fix bosch controls mismatch * bump * bump panda --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index 475a9a3124..42772b49e3 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 475a9a312410908abcaa32f2e48140fcbfc2362f +Subproject commit 42772b49e313688f8f96114806a72784f978e990 From b0f6a8bdf78e3be3bd698ccda3477c6adb2cd270 Mon Sep 17 00:00:00 2001 From: Jason Young <46612682+jyoung8607@users.noreply.github.com> Date: Thu, 9 Jun 2022 12:08:23 -0500 Subject: [PATCH 18/93] VW MQB: Add FW for 2020 Volkswagen Tiguan (#24795) --- selfdrive/car/volkswagen/values.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index 8cba9988bd..9b6f6a16c3 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -564,6 +564,7 @@ FW_VERSIONS = { (Ecu.eps, 0x712, None): [ b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820529A6060603', b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\x0521A60604A1', + b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567A6000600', b'\xf1\x875QF909144B \xf1\x895582\xf1\x82\00571A60634A1', b'\xf1\x875QM909144B \xf1\x891081\xf1\x82\x0521A60604A1', b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\x0521A60604A1', From 5bb1554ec49f4c3e0e29d614c6e8de2b5f71e163 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 9 Jun 2022 10:55:05 -0700 Subject: [PATCH 19/93] build checks do not rely on each other (#24783) --- .github/workflows/prebuilt.yaml | 3 ++- .github/workflows/release.yaml | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/.github/workflows/prebuilt.yaml b/.github/workflows/prebuilt.yaml index a6808a0276..7acc8a2254 100644 --- a/.github/workflows/prebuilt.yaml +++ b/.github/workflows/prebuilt.yaml @@ -25,11 +25,12 @@ jobs: IMAGE_NAME: openpilot-prebuilt steps: - name: Wait for green check mark - uses: lewagon/wait-on-check-action@v0.2 + uses: commaai/wait-on-check-action@f16fc3bb6cd4886520b4e9328db1d42104d5cadc with: ref: master wait-interval: 30 running-workflow-name: 'build prebuilt' + check-regexp: ^((?!.*(build master-ci).*).)*$ - uses: actions/checkout@v3 with: submodules: true diff --git a/.github/workflows/release.yaml b/.github/workflows/release.yaml index 14555f1e7d..fb5a37eeef 100644 --- a/.github/workflows/release.yaml +++ b/.github/workflows/release.yaml @@ -12,11 +12,12 @@ jobs: if: github.repository == 'commaai/openpilot' steps: - name: Wait for green check mark - uses: lewagon/wait-on-check-action@v0.2 + uses: commaai/wait-on-check-action@f16fc3bb6cd4886520b4e9328db1d42104d5cadc with: ref: master wait-interval: 30 running-workflow-name: 'build master-ci' + check-regexp: ^((?!.*(build prebuilt).*).)*$ - uses: actions/checkout@v3 with: submodules: true From 2d0a72a0e837457e5e93dda143e5909889cb03f5 Mon Sep 17 00:00:00 2001 From: Erich Moraga <33645296+ErichMoraga@users.noreply.github.com> Date: Thu, 9 Jun 2022 13:07:17 -0500 Subject: [PATCH 20/93] Add missing CHR engine f/w (#24797) `@papifrio#5178` 2019 Toyota C-HR ICE ("nodsu" TSS-P) DongleID/route ade67da77ee74b16|2022-06-09--00-36-15 --- 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 a9517d26bf..70a94be167 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -558,6 +558,7 @@ FW_VERSIONS = { b'\x033F401100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203102\x00\x00\x00\x00', b'\x033F401200\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00', b'\x033F424000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00', + b'\x033F424000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'8821F0W01000 ', From 074f614f880fa85a56596cd2917c07d8f39961f1 Mon Sep 17 00:00:00 2001 From: AlexandreSato <66435071+AlexandreSato@users.noreply.github.com> Date: Thu, 9 Jun 2022 16:01:31 -0300 Subject: [PATCH 21/93] Lexus: NX Hybrid 2020 support (#24796) * LEXUS: Missing esp and engine FW versions in values.py DongleId: 09ae96064ed85a14 | testRoute: 09ae96064ed85a14|2022-01-10--01-57-37 | * Make a new platform for NX Hybrid TSS2 * Update releases Co-authored-by: Shane Smiskol --- RELEASES.md | 1 + docs/CARS.md | 1 + selfdrive/car/tests/routes.py | 1 + selfdrive/car/toyota/interface.py | 2 +- selfdrive/car/toyota/values.py | 24 ++++++++++++++++++++++-- 5 files changed, 26 insertions(+), 3 deletions(-) diff --git a/RELEASES.md b/RELEASES.md index de57d520d3..87e5adb584 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -6,6 +6,7 @@ Version 0.8.15 (2022-XX-XX) * Effective feedforward that uses road roll * Simplified tuning, all car-specific parameters can be derived from data * AGNOS 5 +* Lexus NX Hybrid 2020 support thanks to AlexandreSato! Version 0.8.14 (2022-06-01) ======================== diff --git a/docs/CARS.md b/docs/CARS.md index 2aa624d849..9a0788d875 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -51,6 +51,7 @@ How We Rate The Cars |Lexus|ES 2019-21|All|||||| |Lexus|ES Hybrid 2019-22|All|||||| |Lexus|NX 2020|All|||||| +|Lexus|NX Hybrid 2020|All|||||| |Lexus|UX Hybrid 2019-21|All|||||| |Toyota|Camry 2021-22|All||[4](#footnotes)|||| |Toyota|Camry Hybrid 2021-22|All|||||| diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index d2ec6b68d7..e7942f517c 100644 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -144,6 +144,7 @@ routes = [ TestRoute("ec429c0f37564e3c|2020-02-01--17-28-12", TOYOTA.LEXUS_NXH), TestRoute("964c09eb11ca8089|2020-11-03--22-04-00", TOYOTA.LEXUS_NX), TestRoute("3fd5305f8b6ca765|2021-04-28--19-26-49", TOYOTA.LEXUS_NX_TSS2), + TestRoute("09ae96064ed85a14|2022-06-09--12-22-31", TOYOTA.LEXUS_NXH_TSS2), TestRoute("0a302ffddbb3e3d3|2020-02-08--16-19-08", TOYOTA.HIGHLANDER_TSS2), TestRoute("437e4d2402abf524|2021-05-25--07-58-50", TOYOTA.HIGHLANDERH_TSS2), TestRoute("3183cd9b021e89ce|2021-05-25--10-34-44", TOYOTA.HIGHLANDER), diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index a1ce8c4980..9440c73f5d 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -186,7 +186,7 @@ class CarInterface(CarInterfaceBase): ret.mass = 3108 * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max set_lat_tune(ret.lateralTuning, LatTunes.PID_M) - elif candidate in (CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.LEXUS_NX_TSS2): + elif candidate in (CAR.LEXUS_NX, CAR.LEXUS_NXH, CAR.LEXUS_NX_TSS2, CAR.LEXUS_NXH_TSS2): stop_and_go = True ret.wheelbase = 2.66 ret.steerRatio = 14.7 diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 70a94be167..412a9c9390 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -77,6 +77,7 @@ class CAR: LEXUS_NX = "LEXUS NX 2018" LEXUS_NXH = "LEXUS NX HYBRID 2018" LEXUS_NX_TSS2 = "LEXUS NX 2020" + LEXUS_NXH_TSS2 = "LEXUS NX HYBRID 2020" LEXUS_RC = "LEXUS RC 2020" LEXUS_RX = "LEXUS RX 2016" LEXUS_RXH = "LEXUS RX HYBRID 2017" @@ -158,6 +159,7 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { CAR.LEXUS_NX: ToyotaCarInfo("Lexus NX 2018-19", footnotes=[Footnote.DSU]), CAR.LEXUS_NXH: ToyotaCarInfo("Lexus NX Hybrid 2018-19", footnotes=[Footnote.DSU]), CAR.LEXUS_NX_TSS2: ToyotaCarInfo("Lexus NX 2020"), + CAR.LEXUS_NXH_TSS2: ToyotaCarInfo("Lexus NX Hybrid 2020"), CAR.LEXUS_RC: ToyotaCarInfo("Lexus RC 2020"), CAR.LEXUS_RX: ToyotaCarInfo("Lexus RX 2016-18", footnotes=[Footnote.DSU]), CAR.LEXUS_RXH: ToyotaCarInfo("Lexus RX Hybrid 2016-19", footnotes=[Footnote.DSU]), @@ -1584,6 +1586,23 @@ FW_VERSIONS = { b'\x028646F7803100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', ], }, + CAR.LEXUS_NXH_TSS2: { + (Ecu.engine, 0x700, None): [ + b'\x0237887000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.esp, 0x7b0, None): [ + b'F152678210\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B78120\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F3301400\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F78030A0\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + ], + }, CAR.LEXUS_NXH: { (Ecu.engine, 0x7e0, None): [ b'\x0237841000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', @@ -1914,6 +1933,7 @@ DBC = { CAR.LEXUS_NXH: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), CAR.LEXUS_NX: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), CAR.LEXUS_NX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.LEXUS_NXH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.PRIUS_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.MIRAI: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.ALPHARD_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), @@ -1926,7 +1946,7 @@ EPS_SCALE = defaultdict(lambda: 73, {CAR.PRIUS: 66, CAR.COROLLA: 88, CAR.LEXUS_I # Toyota/Lexus Safety Sense 2.0 and 2.5 TSS2_CAR = {CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2, - CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2, CAR.AVALONH_TSS2, CAR.ALPHARDH_TSS2} + CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.LEXUS_NXH_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2, CAR.AVALONH_TSS2, CAR.ALPHARDH_TSS2} NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH} @@ -1935,7 +1955,7 @@ RADAR_ACC_CAR = {CAR.RAV4H_TSS2_2022, CAR.RAV4_TSS2_2022} EV_HYBRID_CAR = {CAR.AVALONH_2019, CAR.AVALONH_TSS2, CAR.CAMRYH, CAR.CAMRYH_TSS2, CAR.CHRH, CAR.COROLLAH_TSS2, CAR.HIGHLANDERH, CAR.HIGHLANDERH_TSS2, CAR.PRIUS, CAR.PRIUS_V, CAR.RAV4H, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022, CAR.LEXUS_CTH, CAR.MIRAI, CAR.LEXUS_ESH, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_NXH, CAR.LEXUS_RXH, - CAR.LEXUS_RXH_TSS2, CAR.PRIUS_TSS2, CAR.ALPHARDH_TSS2} + CAR.LEXUS_RXH_TSS2, CAR.LEXUS_NXH_TSS2, CAR.PRIUS_TSS2, CAR.ALPHARDH_TSS2} # no resume button press required NO_STOP_TIMER_CAR = TSS2_CAR | {CAR.PRIUS_V, CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_ESH} From ab8a6352b02b4240f9888af9f3b430c6d6bc4be8 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 9 Jun 2022 15:02:41 -0700 Subject: [PATCH 22/93] some release notes --- RELEASES.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/RELEASES.md b/RELEASES.md index 87e5adb584..f6d40e6c76 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -5,6 +5,12 @@ Version 0.8.15 (2022-XX-XX) * 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 + * Significantly improved control on TSS-P Prius +* New driver monitoring model + * takes a larger input frame + * outputs a driver state for both driver and passenger + * automatically determines which side the driver is on (soon) +* Reduced power usage: device runs cooler and fan spins less * AGNOS 5 * Lexus NX Hybrid 2020 support thanks to AlexandreSato! From 8c101c61a82f4b72939bcae7cc7a651ac8fc6fed Mon Sep 17 00:00:00 2001 From: George Hotz <72895+geohot@users.noreply.github.com> Date: Thu, 9 Jun 2022 15:11:32 -0700 Subject: [PATCH 23/93] EyeSight standard on all 2019+ Ascent and Forester (#24799) * on all 2019+ Ascent Forester * update docs Co-authored-by: Adeeb Shihadeh --- docs/CARS.md | 4 ++-- selfdrive/car/subaru/values.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/docs/CARS.md b/docs/CARS.md index 9a0788d875..20c29e3cb5 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -104,7 +104,7 @@ How We Rate The Cars |Mazda|CX-5 2022|All|||||| |SEAT|Ateca 2018|Driver Assistance|||||| |SEAT|Leon 2014-20|Driver Assistance|||||| -|Subaru|Forester 2019-21|EyeSight|||||| +|Subaru|Forester 2019-21|All|||||| |Škoda|Kamiq 2021[6](#footnotes)|Driver Assistance|||||| |Škoda|Karoq 2019|Driver Assistance|||||| |Škoda|Kodiaq 2018-19|Driver Assistance|||||| @@ -187,7 +187,7 @@ How We Rate The Cars |Nissan|Leaf 2018-22|ProPILOT|||||| |Nissan|Rogue 2018-20|ProPILOT|||||| |Nissan|X-Trail 2017|ProPILOT|||||| -|Subaru|Ascent 2019-20|EyeSight|||||| +|Subaru|Ascent 2019-20|All|||||| |Subaru|Crosstrek 2018-19|EyeSight|||||| |Subaru|Crosstrek 2020-21|EyeSight|||||| |Subaru|Impreza 2017-19|EyeSight|||||| diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 412333edcd..45358eb3a4 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -41,7 +41,7 @@ class SubaruCarInfo(CarInfo): CAR_INFO: Dict[str, Union[SubaruCarInfo, List[SubaruCarInfo]]] = { - CAR.ASCENT: SubaruCarInfo("Subaru Ascent 2019-20"), + CAR.ASCENT: SubaruCarInfo("Subaru Ascent 2019-20", "All"), CAR.IMPREZA: [ SubaruCarInfo("Subaru Impreza 2017-19"), SubaruCarInfo("Subaru Crosstrek 2018-19", video_link="https://youtu.be/Agww7oE1k-s?t=26"), @@ -50,7 +50,7 @@ CAR_INFO: Dict[str, Union[SubaruCarInfo, List[SubaruCarInfo]]] = { SubaruCarInfo("Subaru Impreza 2020-21"), SubaruCarInfo("Subaru Crosstrek 2020-21"), ], - CAR.FORESTER: SubaruCarInfo("Subaru Forester 2019-21"), + CAR.FORESTER: SubaruCarInfo("Subaru Forester 2019-21", "All"), CAR.FORESTER_PREGLOBAL: SubaruCarInfo("Subaru Forester 2017-18"), CAR.LEGACY_PREGLOBAL: SubaruCarInfo("Subaru Legacy 2015-18"), CAR.OUTBACK_PREGLOBAL: SubaruCarInfo("Subaru Outback 2015-17"), From 2ac693100359d9df34d5b2b450a0058f2dd6b7c4 Mon Sep 17 00:00:00 2001 From: ZwX1616 Date: Thu, 9 Jun 2022 15:23:29 -0700 Subject: [PATCH 24/93] fullframe DM: flip RHD yaw to use matching thresholds --- selfdrive/monitoring/driver_monitor.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py index 4affa1e796..e7ed175846 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/selfdrive/monitoring/driver_monitor.py @@ -240,6 +240,8 @@ class DriverStatus(): self.face_detected = driver_data.faceProb > self.settings._FACE_THRESHOLD self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_data.faceOrientation, driver_data.facePosition, cal_rpy) + if self.wheel_on_right: + self.pose.yaw *= -1 self.pose.pitch_std = driver_data.faceOrientationStd[0] self.pose.yaw_std = driver_data.faceOrientationStd[1] model_std_max = max(self.pose.pitch_std, self.pose.yaw_std) From 1fe582686f5446631fa2c87a1c45dcb8f24fdc5e Mon Sep 17 00:00:00 2001 From: Griffin Christenson <36981741+griff12@users.noreply.github.com> Date: Thu, 9 Jun 2022 18:34:05 -0500 Subject: [PATCH 25/93] Toyota: Add missing esp fw version for RAV4_TSS2 (#24793) add fwVersion for RAV4_TSS2 add b'\x01F15260R302\x00\x00\x00\x00\x00\x00' to list of Ecu.esp FwVersions for RAV4_TSS2 --- 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 412a9c9390..91a7fa5d10 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -1254,6 +1254,7 @@ FW_VERSIONS = { b'\x01F15260R220\x00\x00\x00\x00\x00\x00', b'\x01F15260R290\x00\x00\x00\x00\x00\x00', b'\x01F15260R300\x00\x00\x00\x00\x00\x00', + b'\x01F15260R302\x00\x00\x00\x00\x00\x00', b'\x01F152642551\x00\x00\x00\x00\x00\x00', b'\x01F152642561\x00\x00\x00\x00\x00\x00', b'\x01F152642700\x00\x00\x00\x00\x00\x00', From 86ce2f8d4def65d3adb1136b24b75700e953a62f Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 9 Jun 2022 17:39:39 -0700 Subject: [PATCH 26/93] Revert "UI: remove DM icon (#24771)" This reverts commit 4d836c6edd6e0f2639a3e2500794990da339eddc. --- selfdrive/ui/qt/onroad.cc | 10 +++++++++- selfdrive/ui/qt/onroad.h | 5 +++++ selfdrive/ui/ui.cc | 2 +- 3 files changed, 15 insertions(+), 2 deletions(-) diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index e549f62a23..ce61c094bd 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -166,6 +166,7 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) { NvgWindow::NvgWindow(VisionStreamType type, QWidget* parent) : fps_filter(UI_FREQ, 3, 1. / UI_FREQ), CameraViewWidget("camerad", type, true, parent) { engage_img = loadPixmap("../assets/img_chffr_wheel.png", {img_size, img_size}); + dm_img = loadPixmap("../assets/img_driver_face.png", {img_size, img_size}); } void NvgWindow::updateState(const UIState &s) { @@ -185,11 +186,13 @@ void NvgWindow::updateState(const UIState &s) { setProperty("speed", QString::number(std::nearbyint(cur_speed))); setProperty("maxSpeed", maxspeed_str); setProperty("speedUnit", s.scene.is_metric ? "km/h" : "mph"); + setProperty("hideDM", cs.getAlertSize() != cereal::ControlsState::AlertSize::NONE); setProperty("status", s.status); - // update engageability at 2Hz + // update engageability and DM icons at 2Hz if (sm.frame % (UI_FREQ / 2) == 0) { setProperty("engageable", cs.getEngageable() || cs.getEnabled()); + setProperty("dmActive", sm["driverMonitoringState"].getDriverMonitoringState().getIsActiveMode()); } } @@ -231,6 +234,11 @@ void NvgWindow::drawHud(QPainter &p) { engage_img, bg_colors[status], 1.0); } + // dm icon + if (!hideDM) { + drawIcon(p, radius / 2 + (bdr_s * 2), rect().bottom() - footer_h / 2, + dm_img, QColor(0, 0, 0, 70), dmActive ? 1.0 : 0.2); + } p.restore(); } diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index e1f665149e..6ca2b3c738 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -32,6 +32,8 @@ class NvgWindow : public CameraViewWidget { Q_PROPERTY(QString maxSpeed MEMBER maxSpeed); Q_PROPERTY(bool is_cruise_set MEMBER is_cruise_set); Q_PROPERTY(bool engageable MEMBER engageable); + Q_PROPERTY(bool dmActive MEMBER dmActive); + Q_PROPERTY(bool hideDM MEMBER hideDM); Q_PROPERTY(int status MEMBER status); public: @@ -43,6 +45,7 @@ private: void drawText(QPainter &p, int x, int y, const QString &text, int alpha = 255); QPixmap engage_img; + QPixmap dm_img; const int radius = 192; const int img_size = (radius / 2) * 1.5; QString speed; @@ -50,6 +53,8 @@ private: QString maxSpeed; bool is_cruise_set = false; bool engageable = false; + bool dmActive = false; + bool hideDM = false; int status = STATUS_DISENGAGED; protected: diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index f7c2c90437..c1af4ed6f4 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -231,7 +231,7 @@ void UIState::updateStatus() { UIState::UIState(QObject *parent) : QObject(parent) { sm = std::make_unique>({ "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "roadCameraState", - "pandaStates", "carParams", "sensorEvents", "carState", "liveLocationKalman", + "pandaStates", "carParams", "driverMonitoringState", "sensorEvents", "carState", "liveLocationKalman", "wideRoadCameraState", "managerState", "navInstruction", "navRoute", }); From 47f80e7add8635e2d80a395f72906cd40c956459 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 9 Jun 2022 17:44:58 -0700 Subject: [PATCH 27/93] Hyundai: Tucson 2021 support (#24791) * add support for 2021 Hyundai Tucson base configuration and fingerprints * fix fcw again and don't use mando radar for tucson * last fixes for working tucson * Apply suggestions from code review * add to cars * increase steerRatio * missing car info * one platform * rename * add min enable speed * update releases Co-authored-by: bluesforte Co-authored-by: Adeeb Shihadeh --- RELEASES.md | 1 + docs/CARS.md | 1 + selfdrive/car/hyundai/interface.py | 4 ++-- selfdrive/car/hyundai/values.py | 19 +++++++++++++------ selfdrive/car/tests/routes.py | 2 +- 5 files changed, 18 insertions(+), 9 deletions(-) diff --git a/RELEASES.md b/RELEASES.md index f6d40e6c76..acbafd5f5b 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -12,6 +12,7 @@ Version 0.8.15 (2022-XX-XX) * automatically determines which side the driver is on (soon) * Reduced power usage: device runs cooler and fan spins less * AGNOS 5 +* Hyundai Tucson 2021 support thanks to bluesforte! * Lexus NX Hybrid 2020 support thanks to AlexandreSato! Version 0.8.14 (2022-06-01) diff --git a/docs/CARS.md b/docs/CARS.md index 20c29e3cb5..c8614c8037 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -172,6 +172,7 @@ How We Rate The Cars |Hyundai|Ioniq Hybrid 2017-19|SCC + LKAS|||||| |Hyundai|Ioniq Plug-in Hybrid 2019|SCC + LKAS|||||| |Hyundai|Sonata 2018-19|SCC + LKAS|||||| +|Hyundai|Tucson 2021|SCC + LKAS|||||| |Hyundai|Veloster 2019-20|SCC + LKAS|||||| |Jeep|Grand Cherokee 2016-18|Adaptive Cruise|||||| |Jeep|Grand Cherokee 2019-20|Adaptive Cruise|||||| diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index e051079dc4..37f29d23c3 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -170,9 +170,9 @@ class CarInterface(CarInterfaceBase): tire_stiffness_factor = 0.5 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] - elif candidate == CAR.TUCSON_DIESEL_2019: + elif candidate == CAR.TUCSON: ret.lateralTuning.pid.kf = 0.00005 - ret.mass = 3633. * CV.LB_TO_KG + ret.mass = 3520. * CV.LB_TO_KG ret.wheelbase = 2.67 ret.steerRatio = 14.00 * 1.15 tire_stiffness_factor = 0.385 diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 54bbd80475..7d3ab93cde 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -52,7 +52,7 @@ class CAR: SANTA_FE_PHEV_2022 = "HYUNDAI SANTA FE PlUG-IN HYBRID 2022" SONATA = "HYUNDAI SONATA 2020" SONATA_LF = "HYUNDAI SONATA 2019" - TUCSON_DIESEL_2019 = "HYUNDAI TUCSON DIESEL 2019" + TUCSON = "HYUNDAI TUCSON 2019" PALISADE = "HYUNDAI PALISADE 2020" VELOSTER = "HYUNDAI VELOSTER 2019" SONATA_HYBRID = "HYUNDAI SONATA HYBRID 2021" @@ -105,7 +105,10 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = { CAR.SANTA_FE_PHEV_2022: HyundaiCarInfo("Hyundai Santa Fe Plug-in Hybrid 2022", "All", harness=Harness.hyundai_l), CAR.SONATA: HyundaiCarInfo("Hyundai Sonata 2020-22", "All", video_link="https://www.youtube.com/watch?v=ix63r9kE3Fw", harness=Harness.hyundai_a), CAR.SONATA_LF: HyundaiCarInfo("Hyundai Sonata 2018-19", harness=Harness.hyundai_e), - CAR.TUCSON_DIESEL_2019: HyundaiCarInfo("Hyundai Tucson Diesel 2019", harness=Harness.hyundai_l), + CAR.TUCSON: [ + HyundaiCarInfo("Hyundai Tucson 2021", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_l), + HyundaiCarInfo("Hyundai Tucson Diesel 2019", harness=Harness.hyundai_l), + ], CAR.PALISADE: [ HyundaiCarInfo("Hyundai Palisade 2020-21", "All", video_link="https://youtu.be/TAnDqjF4fDY?t=456", harness=Harness.hyundai_h), HyundaiCarInfo("Kia Telluride 2020", harness=Harness.hyundai_h), @@ -508,18 +511,22 @@ FW_VERSIONS = { b'\xf1\x87LAHSGN012918KF10\x98\x88x\x87\x88\x88x\x87\x88\x88\x98\x88\x87w\x88w\x88\x88\x98\x886o\xf6\xff\x98w\x7f\xff3\x00\xf1\x816W3B1051\x00\x00\xf1\x006W351_C2\x00\x006W3B1051\x00\x00TLF0T20NL2H\r\xbdm', ], }, - CAR.TUCSON_DIESEL_2019: { + CAR.TUCSON: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00TL__ FCA F-CUP 1.00 1.01 99110-D3500 ', + b'\xf1\x00TL__ FCA F-CUP 1.00 1.02 99110-D3510 ', ], (Ecu.engine, 0x7e0, None): [ b'\xf1\x8971TLC2NAIDDIR002\xf1\x8271TLC2NAIDDIR002', + b'\xf1\x81606G3051\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00TL MFC AT KOR LHD 1.00 1.02 95895-D3800 180719', + b'\xf1\x00TL MFC AT USA LHD 1.00 1.06 95895-D3800 190107', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x87LBJXAN202299KF22\x87x\x87\x88ww\x87xx\x88\x97\x88\x87\x88\x98x\x88\x99\x98\x89\x87o\xf6\xff\x87w\x7f\xff\x12\x9a\xf1\x81U083\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U083\x00\x00\x00\x00\x00\x00TTL2V20KL1\x8fRn\x8a', + b'\xf1\x87KMLDCU585233TJ20wx\x87\x88x\x88\x98\x89vfwfwwww\x87f\x9f\xff\x98\xff\x7f\xf9\xf7s\xf1\x816T6G4051\x00\x00\xf1\x006T6J0_C2\x00\x006T6G4051\x00\x00TTL4G24NH2\x00\x00\x00\x00', ], }, CAR.SANTA_FE: { @@ -1198,11 +1205,11 @@ CHECKSUM = { FEATURES = { # which message has the gear "use_cluster_gears": {CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA}, - "use_tcu_gears": {CAR.KIA_OPTIMA, CAR.SONATA_LF, CAR.VELOSTER, CAR.TUCSON_DIESEL_2019}, + "use_tcu_gears": {CAR.KIA_OPTIMA, CAR.SONATA_LF, CAR.VELOSTER, CAR.TUCSON}, "use_elect_gears": {CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019}, # these cars use the FCA11 message for the AEB and FCW signals, all others use SCC12 - "use_fca": {CAR.SONATA, CAR.SONATA_HYBRID, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.GENESIS_G70_2020, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.TUCSON_DIESEL_2019}, + "use_fca": {CAR.SONATA, CAR.SONATA_HYBRID, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.GENESIS_G70_2020, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.TUCSON}, } HDA2_CAR = {CAR.KIA_EV6, } @@ -1250,7 +1257,7 @@ DBC = { CAR.SANTA_FE_PHEV_2022: dbc_dict('hyundai_kia_generic', None), CAR.SONATA: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'), CAR.SONATA_LF: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format - CAR.TUCSON_DIESEL_2019: dbc_dict('hyundai_kia_generic', None), + CAR.TUCSON: dbc_dict('hyundai_kia_generic', None), CAR.PALISADE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'), CAR.VELOSTER: dbc_dict('hyundai_kia_generic', None), CAR.KIA_CEED: dbc_dict('hyundai_kia_generic', None), diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index e7942f517c..a9d36f9eab 100644 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -84,7 +84,7 @@ routes = [ TestRoute("c75a59efa0ecd502|2021-03-11--20-52-55", HYUNDAI.KIA_SELTOS), TestRoute("5b7c365c50084530|2020-04-15--16-13-24", HYUNDAI.SONATA), TestRoute("b2a38c712dcf90bd|2020-05-18--18-12-48", HYUNDAI.SONATA_LF), - TestRoute("fb3fd42f0baaa2f8|2022-03-30--15-25-05", HYUNDAI.TUCSON_DIESEL_2019), + TestRoute("fb3fd42f0baaa2f8|2022-03-30--15-25-05", HYUNDAI.TUCSON), TestRoute("5875672fc1d4bf57|2020-07-23--21-33-28", HYUNDAI.KIA_SORENTO), TestRoute("9c917ba0d42ffe78|2020-04-17--12-43-19", HYUNDAI.PALISADE), TestRoute("3f29334d6134fcd4|2022-03-30--22-00-50", HYUNDAI.IONIQ_PHEV_2019), From 1b402687e4bb7146a9aa0c92d8b5062ce53aeb22 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 9 Jun 2022 18:51:34 -0700 Subject: [PATCH 28/93] docs: adjust steering torque thresholds (#24798) * Prius has good steering control now * 1.5 is the threshold for good torque * get back down there, hondas * half stars for 1.0-1.5 * show number of cars * try bigger * emphasize tiers balanced * Add half star * Update ref_commit --- docs/CARS.md | 148 +++++++++++------------ selfdrive/car/CARS_template.md | 5 +- selfdrive/car/docs_definitions.py | 31 ++--- selfdrive/car/honda/interface.py | 4 +- selfdrive/car/toyota/values.py | 9 +- selfdrive/test/process_replay/ref_commit | 2 +- 6 files changed, 100 insertions(+), 99 deletions(-) diff --git a/docs/CARS.md b/docs/CARS.md index c8614c8037..e9b9e947f3 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -25,7 +25,8 @@ How We Rate The Cars - - No steering control below certain speeds. ### Steering Torque -- - Car has enough steering torque for comfortable highway driving. +- - Car has enough steering torque to take tighter turns. +- - Car has enough steering torque for comfortable highway driving. - - Limited ability to make turns. ### Actively Maintained @@ -34,7 +35,7 @@ How We Rate The Cars **All supported cars can move between the tiers as support changes.** -## Gold Cars +# Gold - 29 cars |Make|Model|Supported Package|openpilot ACC|Stop and Go|Steer to 0|Steering Torque|Actively Maintained| |---|---|---|:---:|:---:|:---:|:---:|:---:| @@ -53,6 +54,8 @@ How We Rate The Cars |Lexus|NX 2020|All|||||| |Lexus|NX Hybrid 2020|All|||||| |Lexus|UX Hybrid 2019-21|All|||||| +|Toyota|Avalon 2022|All|||||| +|Toyota|Avalon Hybrid 2022|All|||||| |Toyota|Camry 2021-22|All||[4](#footnotes)|||| |Toyota|Camry Hybrid 2021-22|All|||||| |Toyota|Corolla 2020-22|All|||||| @@ -66,11 +69,17 @@ How We Rate The Cars |Toyota|RAV4 2019-21|All|||||| |Toyota|RAV4 Hybrid 2019-21|All|||||| -## Silver Cars +# Silver - 78 cars |Make|Model|Supported Package|openpilot ACC|Stop and Go|Steer to 0|Steering Torque|Actively Maintained| |---|---|---|:---:|:---:|:---:|:---:|:---:| +|Audi|A3 2014-19|ACC + Lane Assist|||||| +|Audi|A3 Sportback e-tron 2017-18|ACC + Lane Assist|||||| |Audi|Q2 2018|ACC + Lane Assist|||||| +|Audi|Q3 2020-21|ACC + Lane Assist|||||| +|Audi|RS3 2018|ACC + Lane Assist|||||| +|Audi|S3 2015-17|ACC + Lane Assist|||||| +|Chevrolet|Volt 2017-18[1](#footnotes)|Adaptive Cruise|||||| |Genesis|G70 2018|All|||||| |Genesis|G80 2018|All|||||| |Hyundai|Elantra 2021-22|SCC + LKAS|||||| @@ -84,6 +93,7 @@ How We Rate The Cars |Hyundai|Santa Fe 2021-22|All|||||| |Hyundai|Santa Fe Hybrid 2022|All|||||| |Hyundai|Santa Fe Plug-in Hybrid 2022|All|||||| +|Hyundai|Sonata 2018-19|SCC + LKAS|||||| |Hyundai|Tucson Diesel 2019|SCC + LKAS|||||| |Kia|Ceed 2019|SCC + LKAS|||||| |Kia|Forte 2018|SCC + LKAS|||||| @@ -99,13 +109,13 @@ How We Rate The Cars |Lexus|ES Hybrid 2017-18|LSS|[3](#footnotes)||||| |Lexus|NX 2018-19|All|[3](#footnotes)||||| |Lexus|NX Hybrid 2018-19|All|[3](#footnotes)||||| -|Lexus|RX 2020-22|All|||||| -|Lexus|RX Hybrid 2020-21|All|||||| +|Lexus|RX 2020-22|All|||||| +|Lexus|RX Hybrid 2020-21|All|||||| |Mazda|CX-5 2022|All|||||| |SEAT|Ateca 2018|Driver Assistance|||||| |SEAT|Leon 2014-20|Driver Assistance|||||| |Subaru|Forester 2019-21|All|||||| -|Škoda|Kamiq 2021[6](#footnotes)|Driver Assistance|||||| +|Škoda|Kamiq 2021[5](#footnotes)|Driver Assistance|||||| |Škoda|Karoq 2019|Driver Assistance|||||| |Škoda|Kodiaq 2018-19|Driver Assistance|||||| |Škoda|Octavia 2015, 2018-19|Driver Assistance|||||| @@ -114,75 +124,82 @@ How We Rate The Cars |Škoda|Superb 2015-18|Driver Assistance|||||| |Toyota|Alphard 2019-20|All|||||| |Toyota|Alphard Hybrid 2021|All|||||| -|Toyota|Avalon 2022|All|||||| -|Toyota|Avalon Hybrid 2022|All|||||| +|Toyota|Avalon 2019-21|TSS-P|[3](#footnotes)||||| +|Toyota|Avalon Hybrid 2019-21|TSS-P|[3](#footnotes)||||| |Toyota|Camry 2018-20|All||[4](#footnotes)|||| |Toyota|Camry Hybrid 2018-20|All||[4](#footnotes)|||| +|Toyota|Highlander 2017-19|All|[3](#footnotes)||||| +|Toyota|Highlander Hybrid 2017-19|All|[3](#footnotes)||||| +|Toyota|Prius 2016-20|TSS-P|[3](#footnotes)||||| +|Toyota|Prius Prime 2017-20|All|[3](#footnotes)||||| |Toyota|RAV4 2022|All|||||| +|Toyota|RAV4 Hybrid 2016-18|TSS-P|[3](#footnotes)||||| |Toyota|RAV4 Hybrid 2022|All|||||| -|Volkswagen|Arteon 2018, 2021[8](#footnotes)|Driver Assistance|||||| -|Volkswagen|Passat 2015-19[7](#footnotes)|Driver Assistance|||||| +|Toyota|Sienna 2018-20|All|[3](#footnotes)||||| +|Volkswagen|Arteon 2018, 2021[7](#footnotes)|Driver Assistance|||||| +|Volkswagen|e-Golf 2014, 2018-20|Driver Assistance|||||| +|Volkswagen|Golf 2015-20|Driver Assistance|||||| +|Volkswagen|Golf Alltrack 2017-18|Driver Assistance|||||| +|Volkswagen|Golf GTE 2016|Driver Assistance|||||| +|Volkswagen|Golf GTI 2018-21|Driver Assistance|||||| +|Volkswagen|Golf R 2016-19|Driver Assistance|||||| +|Volkswagen|Golf SportsVan 2016|Driver Assistance|||||| +|Volkswagen|Golf SportWagen 2015|Driver Assistance|||||| +|Volkswagen|Passat 2015-19[6](#footnotes)|Driver Assistance|||||| |Volkswagen|Polo 2020|Driver Assistance|||||| -|Volkswagen|T-Cross 2021[8](#footnotes)|Driver Assistance|||||| -|Volkswagen|T-Roc 2021[8](#footnotes)|Driver Assistance|||||| -|Volkswagen|Taos 2022[8](#footnotes)|Driver Assistance|||||| +|Volkswagen|T-Cross 2021[7](#footnotes)|Driver Assistance|||||| +|Volkswagen|T-Roc 2021[7](#footnotes)|Driver Assistance|||||| +|Volkswagen|Taos 2022[7](#footnotes)|Driver Assistance|||||| |Volkswagen|Touran 2017|Driver Assistance|||||| -## Bronze Cars +# Bronze - 66 cars |Make|Model|Supported Package|openpilot ACC|Stop and Go|Steer to 0|Steering Torque|Actively Maintained| |---|---|---|:---:|:---:|:---:|:---:|:---:| |Acura|ILX 2016-19|AcuraWatch Plus|||||| |Acura|RDX 2016-18|AcuraWatch Plus|||||| -|Acura|RDX 2019-21|All|||||| -|Audi|A3 2014-19|ACC + Lane Assist|||||| -|Audi|A3 Sportback e-tron 2017-18|ACC + Lane Assist|||||| -|Audi|Q3 2020-21|ACC + Lane Assist|||||| -|Audi|RS3 2018|ACC + Lane Assist|||||| -|Audi|S3 2015-17|ACC + Lane Assist|||||| +|Acura|RDX 2019-21|All|||||| |Cadillac|Escalade ESV 2016[1](#footnotes)|ACC + LKAS|||||| -|Chevrolet|Volt 2017-18[1](#footnotes)|Adaptive Cruise|||||| -|Chrysler|Pacifica 2017-18|Adaptive Cruise|||||| +|Chrysler|Pacifica 2017-18|Adaptive Cruise|||||| |Chrysler|Pacifica 2020|Adaptive Cruise|||||| -|Chrysler|Pacifica Hybrid 2017-18|Adaptive Cruise|||||| -|Chrysler|Pacifica Hybrid 2019-21|Adaptive Cruise|||||| +|Chrysler|Pacifica Hybrid 2017-18|Adaptive Cruise|||||| +|Chrysler|Pacifica Hybrid 2019-21|Adaptive Cruise|||||| |Genesis|G90 2018|All|||||| -|GMC|Acadia 2018[1](#footnotes)|Adaptive Cruise|||||| -|Honda|Accord 2018-21|All|||||| -|Honda|Accord Hybrid 2018-21|All|||||| +|GMC|Acadia 2018[1](#footnotes)|Adaptive Cruise|||||| +|Honda|Accord 2018-21|All|||||| +|Honda|Accord Hybrid 2018-21|All|||||| |Honda|Civic 2016-18|Honda Sensing|||||| -|Honda|Civic 2019-20|All|||[2](#footnotes)||| -|Honda|Civic Hatchback 2017-21|Honda Sensing|||||| +|Honda|Civic 2019-20|All|||[2](#footnotes)||| +|Honda|Civic Hatchback 2017-21|Honda Sensing|||||| |Honda|CR-V 2015-16|Touring|||||| -|Honda|CR-V 2017-21|Honda Sensing|||||| +|Honda|CR-V 2017-21|Honda Sensing|||||| |Honda|CR-V Hybrid 2017-19|Honda Sensing|||||| |Honda|e 2020|All|||||| -|Honda|Fit 2018-19|Honda Sensing|||||| +|Honda|Fit 2018-19|Honda Sensing|||||| |Honda|Freed 2020|Honda Sensing|||||| |Honda|HR-V 2019-20|Honda Sensing|||||| -|Honda|Insight 2019-21|All|||||| -|Honda|Inspire 2018|All|||||| +|Honda|Insight 2019-21|All|||||| +|Honda|Inspire 2018|All|||||| |Honda|Odyssey 2018-20|Honda Sensing|||||| -|Honda|Passport 2019-21|All|||||| -|Honda|Pilot 2016-21|Honda Sensing|||||| -|Honda|Ridgeline 2017-22|Honda Sensing|||||| +|Honda|Passport 2019-21|All|||||| +|Honda|Pilot 2016-21|Honda Sensing|||||| +|Honda|Ridgeline 2017-22|Honda Sensing|||||| |Hyundai|Elantra 2017-19|SCC + LKAS|||||| |Hyundai|Genesis 2015-16|SCC + LKAS|||||| |Hyundai|Ioniq Electric 2019|SCC + LKAS|||||| |Hyundai|Ioniq Hybrid 2017-19|SCC + LKAS|||||| |Hyundai|Ioniq Plug-in Hybrid 2019|SCC + LKAS|||||| -|Hyundai|Sonata 2018-19|SCC + LKAS|||||| |Hyundai|Tucson 2021|SCC + LKAS|||||| |Hyundai|Veloster 2019-20|SCC + LKAS|||||| -|Jeep|Grand Cherokee 2016-18|Adaptive Cruise|||||| -|Jeep|Grand Cherokee 2019-20|Adaptive Cruise|||||| +|Jeep|Grand Cherokee 2016-18|Adaptive Cruise|||||| +|Jeep|Grand Cherokee 2019-20|Adaptive Cruise|||||| |Kia|Niro Plug-in Hybrid 2019|SCC + LKAS|||||| |Kia|Optima 2017|SCC + LKAS|||||| |Lexus|CT Hybrid 2017-18|LSS|[3](#footnotes)||||| |Lexus|IS 2017-19|All|||||| |Lexus|RC 2020|All|||||| -|Lexus|RX 2016-18|All|[3](#footnotes)||||| -|Lexus|RX Hybrid 2016-19|All|[3](#footnotes)||||| +|Lexus|RX 2016-18|All|[3](#footnotes)||||| +|Lexus|RX Hybrid 2016-19|All|[3](#footnotes)||||| |Mazda|CX-9 2021|All|||||| |Nissan|Altima 2019-20|ProPILOT|||||| |Nissan|Leaf 2018-22|ProPILOT|||||| @@ -190,37 +207,21 @@ How We Rate The Cars |Nissan|X-Trail 2017|ProPILOT|||||| |Subaru|Ascent 2019-20|All|||||| |Subaru|Crosstrek 2018-19|EyeSight|||||| -|Subaru|Crosstrek 2020-21|EyeSight|||||| +|Subaru|Crosstrek 2020-21|EyeSight|||||| |Subaru|Impreza 2017-19|EyeSight|||||| -|Subaru|Impreza 2020-21|EyeSight|||||| -|Toyota|Avalon 2016-18|TSS-P|[3](#footnotes)||||| -|Toyota|Avalon 2019-21|TSS-P|[3](#footnotes)||||| -|Toyota|Avalon Hybrid 2019-21|TSS-P|[3](#footnotes)||||| -|Toyota|C-HR 2017-21|All|||||| -|Toyota|C-HR Hybrid 2017-19|All|||||| +|Subaru|Impreza 2020-21|EyeSight|||||| +|Toyota|Avalon 2016-18|TSS-P|[3](#footnotes)||||| +|Toyota|C-HR 2017-21|All|||||| +|Toyota|C-HR Hybrid 2017-19|All|||||| |Toyota|Corolla 2017-19|All|[3](#footnotes)||||| -|Toyota|Highlander 2017-19|All|[3](#footnotes)||||| -|Toyota|Highlander Hybrid 2017-19|All|[3](#footnotes)||||| -|Toyota|Prius 2016-20|TSS-P|[3](#footnotes)|||[5](#footnotes)|| -|Toyota|Prius Prime 2017-20|All|[3](#footnotes)|||[5](#footnotes)|| -|Toyota|Prius v 2017|TSS-P|[3](#footnotes)|||[5](#footnotes)|| -|Toyota|RAV4 2016-18|TSS-P|[3](#footnotes)||||| -|Toyota|RAV4 Hybrid 2016-18|TSS-P|[3](#footnotes)||||| -|Toyota|Sienna 2018-20|All|[3](#footnotes)||||| -|Volkswagen|Atlas 2018-19, 2022[8](#footnotes)|Driver Assistance|||||| -|Volkswagen|California 2021[8](#footnotes)|Driver Assistance|||||| -|Volkswagen|Caravelle 2020[8](#footnotes)|Driver Assistance|||||| -|Volkswagen|e-Golf 2014, 2018-20|Driver Assistance|||||| -|Volkswagen|Golf 2015-20|Driver Assistance|||||| -|Volkswagen|Golf Alltrack 2017-18|Driver Assistance|||||| -|Volkswagen|Golf GTE 2016|Driver Assistance|||||| -|Volkswagen|Golf GTI 2018-21|Driver Assistance|||||| -|Volkswagen|Golf R 2016-19|Driver Assistance|||||| -|Volkswagen|Golf SportsVan 2016|Driver Assistance|||||| -|Volkswagen|Golf SportWagen 2015|Driver Assistance|||||| -|Volkswagen|Jetta 2018-21|Driver Assistance|||||| -|Volkswagen|Jetta GLI 2021|Driver Assistance|||||| -|Volkswagen|Tiguan 2019-22[8](#footnotes)|Driver Assistance|||||| +|Toyota|Prius v 2017|TSS-P|[3](#footnotes)||||| +|Toyota|RAV4 2016-18|TSS-P|[3](#footnotes)||||| +|Volkswagen|Atlas 2018-19, 2022[7](#footnotes)|Driver Assistance|||||| +|Volkswagen|California 2021[7](#footnotes)|Driver Assistance|||||| +|Volkswagen|Caravelle 2020[7](#footnotes)|Driver Assistance|||||| +|Volkswagen|Jetta 2018-21|Driver Assistance|||||| +|Volkswagen|Jetta GLI 2021|Driver Assistance|||||| +|Volkswagen|Tiguan 2019-22[7](#footnotes)|Driver Assistance|||||| @@ -228,10 +229,9 @@ How We Rate The Cars 22019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph.
3When disconnecting the Driver Support Unit (DSU), openpilot Adaptive Cruise Control (ACC) will replace stock Adaptive Cruise Control (ACC). NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).
428mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
-5An inaccurate steering wheel angle sensor makes precise control difficult.
-6Not including the China market Kamiq, which is based on the (currently) unsupported PQ34 platform.
-7Not including the USA/China market Passat, which is based on the (currently) unsupported PQ35/NMS platform.
-8Model-years 2021 and beyond may have a new camera harness design, which isn't yet available from the comma store. Before ordering, remove the Lane Assist camera cover and check to see if the connector is black (older design) or light brown (newer design). For the newer design, in the interim, choose "VW J533 Development" from the vehicle drop-down for a harness that integrates at the CAN gateway inside the dashboard.
+5Not including the China market Kamiq, which is based on the (currently) unsupported PQ34 platform.
+6Not including the USA/China market Passat, which is based on the (currently) unsupported PQ35/NMS platform.
+7Model-years 2021 and beyond may have a new camera harness design, which isn't yet available from the comma store. Before ordering, remove the Lane Assist camera cover and check to see if the connector is black (older design) or light brown (newer design). For the newer design, in the interim, choose "VW J533 Development" from the vehicle drop-down for a harness that integrates at the CAN gateway inside the dashboard.
## Community Maintained Cars Although they're not upstream, the community has openpilot running on other makes and models. See the 'Community Supported Models' section of each make [on our wiki](https://wiki.comma.ai/). \ No newline at end of file diff --git a/selfdrive/car/CARS_template.md b/selfdrive/car/CARS_template.md index cd9b7bc6ee..f93dc64b06 100644 --- a/selfdrive/car/CARS_template.md +++ b/selfdrive/car/CARS_template.md @@ -28,7 +28,8 @@ How We Rate The Cars - {{star_icon.format(Star.EMPTY.value)}} - No steering control below certain speeds. ### Steering Torque -- {{star_icon.format(Star.FULL.value)}} - Car has enough steering torque for comfortable highway driving. +- {{star_icon.format(Star.FULL.value)}} - Car has enough steering torque to take tighter turns. +- {{star_icon.format(Star.HALF.value)}} - Car has enough steering torque for comfortable highway driving. - {{star_icon.format(Star.EMPTY.value)}} - Limited ability to make turns. ### Actively Maintained @@ -38,7 +39,7 @@ How We Rate The Cars **All supported cars can move between the tiers as support changes.** {% for tier, cars in tiers.items() %} -## {{tier.name.title()}} Cars +# {{tier.name.title()}} - {{cars | length}} cars |{{Column | map(attribute='value') | join('|')}}| |---|---|---|:---:|:---:|:---:|:---:|:---:| diff --git a/selfdrive/car/docs_definitions.py b/selfdrive/car/docs_definitions.py index 95561872ac..e3b46c29e4 100644 --- a/selfdrive/car/docs_definitions.py +++ b/selfdrive/car/docs_definitions.py @@ -6,7 +6,9 @@ from dataclasses import dataclass from enum import Enum from typing import Dict, List, Optional, Union, no_type_check -STEERING_TORQUE_THRESHOLD = 2.0 # m/s^2 +TACO_TORQUE_THRESHOLD = 2.5 # m/s^2 +GREAT_TORQUE_THRESHOLD = 1.5 # m/s^2 +GOOD_TORQUE_THRESHOLD = 1.0 # m/s^2 class Tier(Enum): @@ -70,11 +72,6 @@ class CarInfo: if self.min_enable_speed is not None: min_enable_speed = self.min_enable_speed - # TODO: remove hardcoded good torque and just use maxLateralAccel - good_torque = self.good_torque - if not math.isnan(CP.maxLateralAccel): - good_torque = CP.maxLateralAccel >= STEERING_TORQUE_THRESHOLD - self.car_name = CP.carName self.make, self.model = self.name.split(' ', 1) self.row = { @@ -82,21 +79,27 @@ class CarInfo: Column.MODEL: self.model, Column.PACKAGE: self.package, # StarColumns - Column.LONGITUDINAL: CP.openpilotLongitudinalControl and not CP.radarOffCan, - Column.FSR_LONGITUDINAL: min_enable_speed <= 0., - Column.FSR_STEERING: min_steer_speed <= 0., - Column.STEERING_TORQUE: good_torque, - Column.MAINTAINED: CP.carFingerprint not in non_tested_cars and self.harness is not Harness.none, + Column.LONGITUDINAL: Star.FULL if CP.openpilotLongitudinalControl and not CP.radarOffCan else Star.EMPTY, + Column.FSR_LONGITUDINAL: Star.FULL if min_enable_speed <= 0. else Star.EMPTY, + Column.FSR_STEERING: Star.FULL if min_steer_speed <= 0. else Star.EMPTY, + Column.STEERING_TORQUE: Star.FULL if self.good_torque else Star.EMPTY, # TODO: remove hardcoding and use maxLateralAccel + Column.MAINTAINED: Star.FULL if CP.carFingerprint not in non_tested_cars and self.harness is not Harness.none else Star.EMPTY, } + if not math.isnan(CP.maxLateralAccel): + if CP.maxLateralAccel >= GREAT_TORQUE_THRESHOLD: + self.row[Column.STEERING_TORQUE] = Star.FULL + elif CP.maxLateralAccel >= GOOD_TORQUE_THRESHOLD: + self.row[Column.STEERING_TORQUE] = Star.HALF + else: + self.row[Column.STEERING_TORQUE] = Star.EMPTY + if CP.notCar: for col in StarColumns: - self.row[col] = True + self.row[col] = Star.FULL self.all_footnotes = all_footnotes for column in StarColumns: - self.row[column] = Star.FULL if self.row[column] else Star.EMPTY - # Demote if footnote specifies a star footnote = get_footnote(self.footnotes, column) if footnote is not None and footnote.value.star is not None: diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index ad98f8863f..de5db98ea6 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -100,7 +100,7 @@ class CarInterface(CarInterfaceBase): ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 8000], [0, 2560, 3840]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.1]] else: - ret.maxLateralAccel = 1.7 + ret.maxLateralAccel = 0.4 ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.1], [0.33]] tire_stiffness_factor = 1. @@ -245,7 +245,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 14.35 # as spec ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.82 - ret.maxLateralAccel = 1.5 + ret.maxLateralAccel = 0.8 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]] elif candidate == CAR.ODYSSEY_CHN: diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 91a7fa5d10..7e6cfa0d09 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -93,9 +93,6 @@ class Footnote(Enum): CAMRY = CarFootnote( "28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.", Column.FSR_LONGITUDINAL) - ANGLE_SENSOR = CarFootnote( - "An inaccurate steering wheel angle sensor makes precise control difficult.", - Column.STEERING_TORQUE, star=Star.HALF) @dataclass @@ -133,10 +130,10 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { CAR.HIGHLANDERH: ToyotaCarInfo("Toyota Highlander Hybrid 2017-19", footnotes=[Footnote.DSU]), CAR.HIGHLANDERH_TSS2: ToyotaCarInfo("Toyota Highlander Hybrid 2020-22"), CAR.PRIUS: [ - ToyotaCarInfo("Toyota Prius 2016-20", "TSS-P", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0", footnotes=[Footnote.DSU, Footnote.ANGLE_SENSOR]), - ToyotaCarInfo("Toyota Prius Prime 2017-20", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0", footnotes=[Footnote.DSU, Footnote.ANGLE_SENSOR]), + ToyotaCarInfo("Toyota Prius 2016-20", "TSS-P", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0", footnotes=[Footnote.DSU]), + ToyotaCarInfo("Toyota Prius Prime 2017-20", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0", footnotes=[Footnote.DSU]), ], - CAR.PRIUS_V: ToyotaCarInfo("Toyota Prius v 2017", "TSS-P", min_enable_speed=MIN_ACC_SPEED, footnotes=[Footnote.DSU, Footnote.ANGLE_SENSOR]), + CAR.PRIUS_V: ToyotaCarInfo("Toyota Prius v 2017", "TSS-P", min_enable_speed=MIN_ACC_SPEED, footnotes=[Footnote.DSU]), CAR.PRIUS_TSS2: [ ToyotaCarInfo("Toyota Prius 2021-22", video_link="https://www.youtube.com/watch?v=J58TvCpUd4U"), ToyotaCarInfo("Toyota Prius Prime 2021-22", video_link="https://www.youtube.com/watch?v=J58TvCpUd4U"), diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 3a9b1d9479..2f42570934 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -0e18bb3317437f2cad6d0a5782a9222eda655d58 +f301bdf52b271c8c4ef0f2bce3be9c8f90037652 From 53724e8b6cbcfc6c9d49ebd0834624fa23d0f6e9 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 9 Jun 2022 19:29:54 -0700 Subject: [PATCH 29/93] bypass HDA2 dashcam with file (#24803) Co-authored-by: Comma Device --- selfdrive/car/hyundai/interface.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 37f29d23c3..831b318d55 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +import os from cereal import car from panda import Panda from common.conversions import Conversions as CV @@ -37,7 +38,9 @@ class CarInterface(CarInterfaceBase): # These cars have been put into dashcam only due to both a lack of users and test coverage. # These cars likely still work fine. Once a user confirms each car works and a test route is # added to selfdrive/car/tests/routes.py, we can remove it from this list. - ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, CAR.ELANTRA_GT_I30} or candidate in HDA2_CAR + ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, CAR.ELANTRA_GT_I30} + if candidate in HDA2_CAR: + ret.dashcamOnly = not os.path.exists('/data/enable-ev6') ret.steerActuatorDelay = 0.1 # Default delay ret.steerRateCost = 0.5 From 60aa553b8c0d8323f276b102ce359262e639861b Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 9 Jun 2022 20:56:05 -0700 Subject: [PATCH 30/93] Honda: use common imperial unit message (#24786) * CAR_SPEED should be on all cars * bump opendbc * clean this up too * it's an or * clean up * comment --- opendbc | 2 +- selfdrive/car/honda/carstate.py | 49 ++++++++++----------------------- 2 files changed, 15 insertions(+), 36 deletions(-) diff --git a/opendbc b/opendbc index 30aacafa10..ec0e1f20ba 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 30aacafa10e72db33ecf5e3b7a6eacffa9390b8b +Subproject commit ec0e1f20bae4c39326036c0061418404ac15ae9e diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 5314fe375e..f5cdc838c4 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -22,9 +22,9 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg): ("STEER_ANGLE_RATE", "STEERING_SENSORS"), ("MOTOR_TORQUE", "STEER_MOTOR_TORQUE"), ("STEER_TORQUE_SENSOR", "STEER_STATUS"), + ("IMPERIAL_UNIT", "CAR_SPEED"), ("LEFT_BLINKER", "SCM_FEEDBACK"), ("RIGHT_BLINKER", "SCM_FEEDBACK"), - ("GEAR", gearbox_msg), ("SEATBELT_DRIVER_LAMP", "SEATBELT_STATUS"), ("SEATBELT_DRIVER_LATCHED", "SEATBELT_STATUS"), ("BRAKE_PRESSED", "POWERTRAIN_DATA"), @@ -35,6 +35,7 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg): ("BRAKE_HOLD_ACTIVE", "VSA_STATUS"), ("STEER_STATUS", "STEER_STATUS"), ("GEAR_SHIFTER", gearbox_msg), + ("GEAR", gearbox_msg), ("PEDAL_GAS", "POWERTRAIN_DATA"), ("CRUISE_SETTING", "SCM_BUTTONS"), ("ACC_STATUS", "POWERTRAIN_DATA"), @@ -48,9 +49,10 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg): ("SEATBELT_STATUS", 10), ("CRUISE", 10), ("POWERTRAIN_DATA", 100), + ("CAR_SPEED", 10), ("VSA_STATUS", 50), ("STEER_STATUS", 100), - ("STEER_MOTOR_TORQUE", 0), # TODO: not on every car + ("STEER_MOTOR_TORQUE", 0), # TODO: not on every car ] if CP.carFingerprint == CAR.ODYSSEY_CHN: @@ -73,16 +75,11 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg): signals.append(("BRAKE_PRESSED", "BRAKE_MODULE")) checks.append(("BRAKE_MODULE", 50)) - if CP.carFingerprint in HONDA_BOSCH: - signals += [ - ("EPB_STATE", "EPB_STATUS"), - ("IMPERIAL_UNIT", "CAR_SPEED"), - ] - checks += [ - ("EPB_STATUS", 50), - ("CAR_SPEED", 10), - ] + if CP.carFingerprint in (HONDA_BOSCH | {CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN}): + signals.append(("EPB_STATE", "EPB_STATUS")) + checks.append(("EPB_STATUS", 50)) + if CP.carFingerprint in HONDA_BOSCH: if not CP.openpilotLongitudinalControl: signals += [ ("CRUISE_CONTROL_LABEL", "ACC_HUD"), @@ -121,17 +118,6 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg): ("STANDSTILL", 50), ] - if CP.carFingerprint == CAR.CIVIC: - signals += [("IMPERIAL_UNIT", "HUD_SETTING"), - ("EPB_STATE", "EPB_STATUS")] - checks += [ - ("HUD_SETTING", 50), - ("EPB_STATUS", 50), - ] - elif CP.carFingerprint in (CAR.ODYSSEY, CAR.ODYSSEY_CHN): - signals.append(("EPB_STATE", "EPB_STATUS")) - checks.append(("EPB_STATUS", 50)) - # add gas interceptor reading if we are using it if CP.enableGasInterceptor: signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR")) @@ -179,6 +165,11 @@ class CarState(CarStateBase): # update prevs, update must run once per loop self.prev_cruise_buttons = self.cruise_buttons self.prev_cruise_setting = self.cruise_setting + self.cruise_setting = cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"] + self.cruise_buttons = cp.vl["SCM_BUTTONS"]["CRUISE_BUTTONS"] + + # used for car hud message + self.is_metric = not cp.vl["CAR_SPEED"]["IMPERIAL_UNIT"] # ******************* parse out can ******************* # STANDSTILL->WHEELS_MOVING bit can be noisy around zero, so use XMISSION_SPEED @@ -219,16 +210,12 @@ class CarState(CarStateBase): ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE"] ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE_RATE"] - self.cruise_setting = cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"] - self.cruise_buttons = cp.vl["SCM_BUTTONS"]["CRUISE_BUTTONS"] - ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk( 250, cp.vl["SCM_FEEDBACK"]["LEFT_BLINKER"], cp.vl["SCM_FEEDBACK"]["RIGHT_BLINKER"]) ret.brakeHoldActive = cp.vl["VSA_STATUS"]["BRAKE_HOLD_ACTIVE"] == 1 # TODO: set for all cars - if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, - CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E): + if self.CP.carFingerprint in (HONDA_BOSCH | {CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN}): ret.parkingBrake = cp.vl["EPB_STATUS"]["EPB_STATE"] != 0 gear = int(cp.vl[self.gearbox_msg]["GEAR_SHIFTER"]) @@ -281,14 +268,6 @@ class CarState(CarStateBase): if ret.brake > 0.1: ret.brakePressed = True - # TODO: discover the CAN msg that has the imperial unit bit for all other cars - if self.CP.carFingerprint in (CAR.CIVIC, ): - self.is_metric = not cp.vl["HUD_SETTING"]["IMPERIAL_UNIT"] - elif self.CP.carFingerprint in HONDA_BOSCH: - self.is_metric = not cp.vl["CAR_SPEED"]["IMPERIAL_UNIT"] - else: - self.is_metric = False - if self.CP.carFingerprint in HONDA_BOSCH: ret.stockAeb = (not self.CP.openpilotLongitudinalControl) and bool(cp.vl["ACC_CONTROL"]["AEB_STATUS"] and cp.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5) else: From aeb2c67c904d9ed3a579e6ee40b05606b92713af Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 10 Jun 2022 15:39:18 -0700 Subject: [PATCH 31/93] add PyQt (#24811) --- Pipfile | 2 + Pipfile.lock | 448 ++++++++++++++++++++++++++++----------------------- 2 files changed, 248 insertions(+), 202 deletions(-) diff --git a/Pipfile b/Pipfile index 1e526a5ec0..3958c890fe 100644 --- a/Pipfile +++ b/Pipfile @@ -84,6 +84,8 @@ urllib3 = "*" utm = "*" websocket_client = "*" hatanaka = "==2.4" +PyQt5 = "==5.15.4" +PyQt5-sip = "==12.9.0" [requires] python_version = "3.8" diff --git a/Pipfile.lock b/Pipfile.lock index b0ec993b2a..df2b3bb853 100644 --- a/Pipfile.lock +++ b/Pipfile.lock @@ -1,7 +1,7 @@ { "_meta": { "hash": { - "sha256": "50d3486820d05997c1e084e70362fa080e0a294faa01c7f1d43219e44f94a80b" + "sha256": "4dbfaf9d7a532e0e9a126f828512a5383a817722a1580ef64b99e2427f366a72" }, "pipfile-spec": 6, "requires": { @@ -146,7 +146,7 @@ "sha256:2857e29ff0d34db842cd7ca3230549d1a697f96ee6d3fb071cfa6c7393832597", "sha256:6881edbebdb17b39b4eaaa821b438bf6eddffb4468cf344f09f89def34a8b1df" ], - "markers": "python_version >= '3'", + "markers": "python_version >= '3.5'", "version": "==2.0.12" }, "click": { @@ -347,7 +347,7 @@ "sha256:84d9dd047ffa80596e0f246e2eab0b391788b0503584e8945f2368256d2735ff", "sha256:9d643ff0a55b762d5cdb124b8eaa99c66322e2157b69160bc32796e824360e6d" ], - "markers": "python_version >= '3'", + "markers": "python_version >= '3.5'", "version": "==3.3" }, "importlib-metadata": { @@ -371,7 +371,7 @@ "sha256:6f62d78e2f89b4500b080fe3a81690850cd254227f27f75c3a0c491a1f351ba7", "sha256:e8443a5e7a020e9d7f97f1d7d9cd17c88bcb3bc7e218bf9cf5095fe550be2951" ], - "markers": "python_version < '4.0' and python_full_version >= '3.6.1'", + "markers": "python_version < '4' and python_full_version >= '3.6.1'", "version": "==5.10.1" }, "itsdangerous": { @@ -687,33 +687,23 @@ }, "protobuf": { "hashes": [ - "sha256:06059eb6953ff01e56a25cd02cca1a9649a75a7e65397b5b9b4e929ed71d10cf", - "sha256:097c5d8a9808302fb0da7e20edf0b8d4703274d140fd25c5edabddcde43e081f", - "sha256:284f86a6207c897542d7e956eb243a36bb8f9564c1742b253462386e96c6b78f", - "sha256:32ca378605b41fd180dfe4e14d3226386d8d1b002ab31c969c366549e66a2bb7", - "sha256:3cc797c9d15d7689ed507b165cd05913acb992d78b379f6014e013f9ecb20996", - "sha256:62f1b5c4cd6c5402b4e2d63804ba49a327e0c386c99b1675c8a0fefda23b2067", - "sha256:69ccfdf3657ba59569c64295b7d51325f91af586f8d5793b734260dfe2e94e2c", - "sha256:6f50601512a3d23625d8a85b1638d914a0970f17920ff39cec63aaef80a93fb7", - "sha256:7403941f6d0992d40161aa8bb23e12575637008a5a02283a930addc0508982f9", - "sha256:755f3aee41354ae395e104d62119cb223339a8f3276a0cd009ffabfcdd46bb0c", - "sha256:77053d28427a29987ca9caf7b72ccafee011257561259faba8dd308fda9a8739", - "sha256:7e371f10abe57cee5021797126c93479f59fccc9693dafd6bd5633ab67808a91", - "sha256:9016d01c91e8e625141d24ec1b20fed584703e527d28512aa8c8707f105a683c", - "sha256:9be73ad47579abc26c12024239d3540e6b765182a91dbc88e23658ab71767153", - "sha256:adc31566d027f45efe3f44eeb5b1f329da43891634d61c75a5944e9be6dd42c9", - "sha256:adfc6cf69c7f8c50fd24c793964eef18f0ac321315439d94945820612849c388", - "sha256:af0ebadc74e281a517141daad9d0f2c5d93ab78e9d455113719a45a49da9db4e", - "sha256:cb29edb9eab15742d791e1025dd7b6a8f6fcb53802ad2f6e3adcb102051063ab", - "sha256:cd68be2559e2a3b84f517fb029ee611546f7812b1fdd0aa2ecc9bc6ec0e4fdde", - "sha256:cdee09140e1cd184ba9324ec1df410e7147242b94b5f8b0c64fc89e38a8ba531", - "sha256:db977c4ca738dd9ce508557d4fce0f5aebd105e158c725beec86feb1f6bc20d8", - "sha256:dd5789b2948ca702c17027c84c2accb552fc30f4622a98ab5c51fcfe8c50d3e7", - "sha256:e250a42f15bf9d5b09fe1b293bdba2801cd520a9f5ea2d7fb7536d4441811d20", - "sha256:ff8d8fa42675249bb456f5db06c00de6c2f4c27a065955917b28c4f15978b9c3" + "sha256:0d4719e724472e296062ba8e82a36d64693fcfdb550d9dff98af70ca79eafe3d", + "sha256:2b35602cb65d53c168c104469e714bf68670335044c38eee3c899d6a8af03ffc", + "sha256:32fff501b6df3050936d1839b80ea5899bf34db24792d223d7640611f67de15a", + "sha256:34400fd76f85bdae9a2e9c1444ea4699c0280962423eff4418765deceebd81b5", + "sha256:3767c64593a49c7ac0accd08ed39ce42744405f0989d468f0097a17496fdbe84", + "sha256:3f2ed842e8ca43b790cb4a101bcf577226e0ded98a6a6ba2d5e12095a08dc4da", + "sha256:52c1e44e25f2949be7ffa7c66acbfea940b0945dd416920231f7cb30ea5ac6db", + "sha256:5d9b5c8270461706973c3871c6fbdd236b51dfe9dab652f1fb6a36aa88287e47", + "sha256:72d357cc4d834cc85bd957e8b8e1f4b64c2eac9ca1a942efeb8eb2e723fca852", + "sha256:79cd8d0a269b714f6b32641f86928c718e8d234466919b3f552bfb069dbb159b", + "sha256:a4c0c6f2f95a559e59a0258d3e4b186f340cbdc5adec5ce1bc06d01972527c88", + "sha256:b309fda192850ac4184ca1777aab9655564bc8d10a9cc98f10e1c8bf11295c22", + "sha256:b3d7d4b4945fe3c001403b6c24442901a5e58c0a3059290f5a63523ed4435f82", + "sha256:c8829092c5aeb61619161269b2f8a2e36fd7cb26abbd9282d3bc453f02769146" ], "markers": "python_version >= '3.7'", - "version": "==3.20.1" + "version": "==4.21.1" }, "psutil": { "hashes": [ @@ -841,11 +831,11 @@ }, "pylint": { "hashes": [ - "sha256:095567c96e19e6f57b5b907e67d265ff535e588fe26b12b5ebe1fc5645b2c731", - "sha256:705c620d388035bdd9ff8b44c5bcdd235bfb49d276d488dd2c8ff1736aa42526" + "sha256:549261e0762c3466cc001024c4419c08252cb8c8d40f5c2c6966fea690e7fe2a", + "sha256:bb71e6d169506de585edea997e48d9ff20c0dc0e2fbc1d166bad6b640120326b" ], "index": "pypi", - "version": "==2.13.9" + "version": "==2.14.1" }, "pyopencl": { "hashes": [ @@ -890,6 +880,53 @@ "index": "pypi", "version": "==2022.1.5" }, + "pyqt5": { + "hashes": [ + "sha256:213bebd51821ed89b4d5b35bb10dbe67564228b3568f463a351a08e8b1677025", + "sha256:2a69597e0dd11caabe75fae133feca66387819fc9bc050f547e5551bce97e5be", + "sha256:883a549382fc22d29a0568f3ef20b38c8e7ab633a59498ac4eb63a3bf36d3fd3", + "sha256:8c0848ba790a895801d5bfd171da31cad3e551dbcc4e59677a3b622de2ceca98", + "sha256:a88526a271e846e44779bb9ad7a738c6d3c4a9d01e15a128ecfc6dd4696393b7" + ], + "index": "pypi", + "version": "==5.15.4" + }, + "pyqt5-qt5": { + "hashes": [ + "sha256:1988f364ec8caf87a6ee5d5a3a5210d57539988bf8e84714c7d60972692e2f4a", + "sha256:750b78e4dba6bdf1607febedc08738e318ea09e9b10aea9ff0d73073f11f6962", + "sha256:76980cd3d7ae87e3c7a33bfebfaee84448fd650bad6840471d6cae199b56e154", + "sha256:9cc7a768b1921f4b982ebc00a318ccb38578e44e45316c7a4a850e953e1dd327" + ], + "version": "==5.15.2" + }, + "pyqt5-sip": { + "hashes": [ + "sha256:055581c6fed44ba4302b70eeb82e979ff70400037358908f251cd85cbb3dbd93", + "sha256:0fc9aefacf502696710b36cdc9fa2a61487f55ee883dbcf2c2a6477e261546f7", + "sha256:42274a501ab4806d2c31659170db14c282b8313d2255458064666d9e70d96206", + "sha256:4347bd81d30c8e3181e553b3734f91658cfbdd8f1a19f254777f906870974e6d", + "sha256:485972daff2fb0311013f471998f8ec8262ea381bded244f9d14edaad5f54271", + "sha256:4f8e05fe01d54275877c59018d8e82dcdd0bc5696053a8b830eecea3ce806121", + "sha256:69a3ad4259172e2b1aa9060de211efac39ddd734a517b1924d9c6c0cc4f55f96", + "sha256:6a8701892a01a5a2a4720872361197cc80fdd5f49c8482d488ddf38c9c84f055", + "sha256:6d5bca2fc222d58e8093ee8a81a6e3437067bb22bc3f86d06ec8be721e15e90a", + "sha256:83c3220b1ca36eb8623ba2eb3766637b19eb0ce9f42336ad8253656d32750c0a", + "sha256:a25b9843c7da6a1608f310879c38e6434331aab1dc2fe6cb65c14f1ecf33780e", + "sha256:ac57d796c78117eb39edd1d1d1aea90354651efac9d3590aac67fa4983f99f1f", + "sha256:b09f4cd36a4831229fb77c424d89635fa937d97765ec90685e2f257e56a2685a", + "sha256:c446971c360a0a1030282a69375a08c78e8a61d568bfd6dab3dcc5cf8817f644", + "sha256:c5216403d4d8d857ec4a61f631d3945e44fa248aa2415e9ee9369ab7c8a4d0c7", + "sha256:d3e4489d7c2b0ece9d203ae66e573939f7f60d4d29e089c9f11daa17cfeaae32", + "sha256:d59af63120d1475b2bf94fe8062610720a9be1e8940ea146c7f42bb449d49067", + "sha256:d85002238b5180bce4b245c13d6face848faa1a7a9e5c6e292025004f2fd619a", + "sha256:d8b2bdff7bbf45bc975c113a03b14fd669dc0c73e1327f02706666a7dd51a197", + "sha256:dd05c768c2b55ffe56a9d49ce6cc77cdf3d53dbfad935258a9e347cbfd9a5850", + "sha256:fc43f2d7c438517ee33e929e8ae77132749c15909afab6aeece5fcf4147ffdb5" + ], + "index": "pypi", + "version": "==12.9.0" + }, "pyserial": { "hashes": [ "sha256:3c77e014170dfffbd816e6ffc205e9842efb10be9f58ec16d3e8675b4925cddb", @@ -954,74 +991,74 @@ }, "pyzmq": { "hashes": [ - "sha256:011a45c846ec69a3671ed15893b74b6ad608800c89ac6d0f0411e2137c6b313d", - "sha256:011f26841dd56ed87e464c98023dbbd4c0b3ab8802a045de3ea83e0187eb8145", - "sha256:0258563bf69f6ca305204354f171e0627a9bf8fe78c9d4f63a5e2447035cbb4b", - "sha256:07d2008e51718fba60641e5d1a0646b222b7929f16f6e7cf0834b8439f42c9e8", - "sha256:0a787f7870cba38d655c68ea7ae14bb6c3e9e19bb618d0c2412513321eeaeb80", - "sha256:0a89b9860d2171bcf674648dc8186db9cf3b773ad3c0610a2c7bf189cf3560b6", - "sha256:0de8a7e13ffacfe33c89acc0d7bfa2f5bde94e3f74b7f1e4d43c97ce17864d77", - "sha256:12a53f5c13edf12547ce495afebdd5ab11c1b67ea078a941b21e13161783741a", - "sha256:12eac2294d48ee27d1eaef7e214acedb394e4c95e3a1f6e4467099b82d58ef73", - "sha256:176be6c348dbec04e8e0d41e810743b7084b73e50954a6fedeeafc65d7fa9290", - "sha256:1d480d48253f61ff90115b8069ed32f51a0907eb19101c4a5ae0b9a5973e40ad", - "sha256:21792f4d0fcc5040978ee211c033e915d8b6608ea8a5b33fe197a04f0d43e991", - "sha256:277b3ebc684b369a57a186a9acf629c1b01247eb04d1105536ef2dae5f61168a", - "sha256:2f227150148e7c3db7ecd8a58500439979f556e15455841a30b6d121755b14bc", - "sha256:34b143751e9b2b89cf9b656081f1b2842a563c4c9ffc8465531875daf546e772", - "sha256:3f3807e81bf51d4c63eb12a21920614e0e840645418e9f2e3b5ffdd5991b3415", - "sha256:3fa7126d532effee452c0ab395ab3cbef1c06fd6870ab7e681f812ba9e685cfa", - "sha256:434044eec7f9df08fc5ca5c9bdd1a4bb08663679d43ebe7b9849713956f4d85f", - "sha256:4d861ae20040afc17adef33053c328667da78d4d3676b2936788fd031665e3a8", - "sha256:536491ad640448f14d8aa2dc497c354a348f216eb23513bf5aa0ac40e2b02577", - "sha256:5619f6598d6fd30778053ae2daa48a7c54029816648b908270b751411fd52e74", - "sha256:591b455546d34bb96aa453dd9666bddb8c81314e23dbf2606f9614acf7e73d9f", - "sha256:5a13171268f05d127e31b4c369b753733f67dbb0d765901ef625a115feb5c7de", - "sha256:5eaf7e0841d3d8d1d92838c8b56f98cb9bf35b14bcbe4efa281e4812ef4be728", - "sha256:6c09e6e5c4baf0959287943dc8170624d739ae555d334e896a94d9de01c7bb21", - "sha256:6e2093a97bf3f6008a4be6b5bae8ae3fc409f18373593bef19dd7b381ab8030c", - "sha256:7b518ad9cdbaaeb1a9da3444797698871ae2eeae34ff9a656d5150d37e1e42a1", - "sha256:7ca7d77f24644298cbe53bc279eb7ca05f3b8637473d392f0c9f34b37f08b49a", - "sha256:7eca5902ff41575d9a26f91fc750018b7eb129600ea600fe69ce852fbdfab4e2", - "sha256:8951830d6a00636b3af478091f9668ecc486f1dad01b975527957fd1d8c31bfd", - "sha256:8c234aefeef034c5d6de452e2af5173a95ea06315b685db703091e6f937a6e60", - "sha256:9273f6d1da1018822f41630fb0f3fe208e8e70e5d5e780795326900cfa22d8b6", - "sha256:9622d9560a6fd8d589816cdcec6946642cb4e070b3f68be1d3779b52cf240f73", - "sha256:9ef2d1476cea927ba33a29f59aa128ce3b174e81083cbd091dd3149af741c85d", - "sha256:9feb7ccd426ff2158ce79f4c87a8a1600ed4f77e65e2fffda2b42638b2bc73e4", - "sha256:a0b8528aefceb787f41ad429f3210a3c6b52e99f85413416e3d0c9e6d035f8ac", - "sha256:a2e4d70d34112997a32c8193fae2579aec854745f8730031e5d84cc579fd98ff", - "sha256:a37f0ec88e220326803084208d80229218b309d728954ab747ab21cca33424aa", - "sha256:a45f5c0477d12df05ef2e2922b49b7c0ae9d0f4ff9b6bb0d666558df0ef37122", - "sha256:a64b9cce166396df5f33894074d6515778d48c63aae5ee1abd86d8bbc5a711d8", - "sha256:a89285fedbeca483a855a77285453e21e4fc86ef0944bc018ef4b3033aa04ad2", - "sha256:a8f40604437ec8010f77f7053fd135ccb202d6ca18329903831087cab8dbdab1", - "sha256:b2a4af5e6fa85ee1743c725b46579f8de0b97024eb5ae1a0b5c5711adc436665", - "sha256:b97dc1273f16f85a38cff6668a07b636ef14e30591039efbfd21f5f91efae964", - "sha256:bdd008629293a0d4f00b516841ac0df89f17a64bc2d83bcfa48212d3f3b3ca1a", - "sha256:be3425dfdb9c46dc62d490fc1a6142a5f3dc6605ebb9048ae675056ef621413c", - "sha256:c2394bb857607494c3750b5040f852a1ad7831d7a7907b6106f0af2c70860cef", - "sha256:cb45b7ea577283b547b907a3389d62ca2eaddaf725fbb79cd360802440fa9c91", - "sha256:cd3f563b98e2a8730c93bdc550f119ae766b2d3da1f0d6a3c7735b59adfa1642", - "sha256:cda55ff0a7566405fb29ca38db1829fecb4c041b8dc3f91754f337bb7b27cbd8", - "sha256:df0b05fa4321b090abe5601dea9b1c8933c06f496588ccb397a0b1f9dfe32ebe", - "sha256:e464e7b1be2216eba54b47256c15bf307ae4a656aa0f73becea7b3e7283c5ac2", - "sha256:e730d490b1421e52b43b1b9f5e1f8c3973499206e188f29b582577531e11033b", - "sha256:e7ae3e520bd182a0cbfff3cc69dda3a2c26f69847e81bd3f090ed04471fc1282", - "sha256:e9631c6a339843e4f95efb80ff9a1bfaaf3d611ba9677a7a5cc61ffb923b4e06", - "sha256:f3daabbe42ca31712e29d906dfa4bf1890341d2fd5178de118bc9977a8d2b23b", - "sha256:fe8807d67456e7cf0e9a33b85e0d05bb9d2977dbdb23977e4cc2b843633618fd" - ], - "index": "pypi", - "version": "==23.0.0" + "sha256:057176dd3f5ccf5aad4abd662d76b6a39bbf799baaf2f39cd4fdaf2eab326e43", + "sha256:05ec90a8da618f2398f9d1aa20b18a9ef332992c6ac23e8c866099faad6ef0d6", + "sha256:154de02b15422af28b53d29a02de72121ba503634955017255573fc1f995143d", + "sha256:16b832adb5d8716f46051da5533c480250bf126984ce86804db6137a3a7f931b", + "sha256:1df26aa854bdd3a8341bf199064dd6aa6e240f2eaa3c9fa8d217e5d8b868c73e", + "sha256:28f9164fb2658b7b414fa0894c75b1a9c61375774cdc1bdb7298beb042a2cd87", + "sha256:2951c29b8649f3672af9dca8ff61d86310d3664d9629788b1c66422fb13b1239", + "sha256:2b08774057ae7ce8a2eb4e7d54db05358234440706ce43a85814500c5d7bd22e", + "sha256:2e2ac40f7a91c740ec68d6db07ae19ea9259c959333c68bee56ab2c799a67d66", + "sha256:312e56799410c34797417a4060a8bd37d4db1f06d1ec0c54f7c8fd81e0d90376", + "sha256:38f778a74e3889392e949326cfd0e9b2eb37dcbb2980d98fad2c51703d523db2", + "sha256:3955dd5bbbe02f454655296ee36a66c334c7102a29b8458223d168c0380edfd5", + "sha256:425ba851a6f9892bde1da2024d82e2fe6796bd77e3391fb96665c50fe9d4c6a5", + "sha256:48bbc2db041ab28eeee4a3e8ada0ed336640946dd5a8e53dbd3805f9dbdcf0dc", + "sha256:4fbcd657cda75574fd1315a4c44bd322bc2e219039fb09f146bbe6f8aef039e9", + "sha256:523ba7fd4d8fe75ad09c1e574a648892b75a97d0cfc8005727681053ac19555b", + "sha256:53b2c1326c2e484d450932d2be739f064b7cb572faabec38386098a28516a529", + "sha256:540d7146c3cdc9bbffab039ea067f494eba24d1abe5bd33eb9f963c01e3305d4", + "sha256:563d4281c4dbdf647d93114420151d33f895afc4c46b7115a67a0aa5347e6624", + "sha256:67a049bcf967a39993858beed873ed3405536019820922d4efacfe35ab3da51a", + "sha256:67ec63ae3c9c1fa2e077fcb42e77035e2121a04f987464bdf9945a28535d30ad", + "sha256:68e22c5d3be451e87d47f956b397a7823bfbde2176341bc902fba30f96831d7e", + "sha256:6ab4b6108e69f63c917cd7ef7217c5727955b1ac90600e44a13ed5312019a014", + "sha256:6bd7f18bd4cf51ea8d7e54825902cf36f9d2f35cc51ef618373988d5398b8dd0", + "sha256:6cd53e861bccc0bdc4620f68fb4a91d5bcfe9f4213cf8e200fa498044d33a6dc", + "sha256:6d346e551fa64b89d57a4ac74b9bc66703413f02f50093e089e861999ec5cccc", + "sha256:6ff8708fabc9f9bc2949f457d39b4088c9656c4c9ac15fbbbbaafce8f6d07833", + "sha256:7626e8384275a7dea6f3d1f749fb5e00299042e9c895fc3dbe24cb154909c242", + "sha256:7e7346b2b33dcd4a2171dd8a9870ae283eec8f6231dcbcf237a0f41e74751a50", + "sha256:81623c67cb71b93b5f7e06c9107f3781738ae86866db830c950223d87af2a235", + "sha256:83f1c76068faf62c32a36dd62dc4db642c2027bbbd960f8f6345b59e9d4dc472", + "sha256:8679bb1dd723ecbea03b1f96c98972815775fd8ec756c440a14f289c436c472e", + "sha256:86fb683cb9a9c0bb7476988b7957393ecdd22777d87d804442c66e62c99197f9", + "sha256:8757c62f7960cd26122f7aaaf86eda1e016fa85734c3777b8054dd334d7dea4d", + "sha256:894be7d17228e7328cc188096c0162697211ec91761f6812fff12790cbe11c66", + "sha256:8a0f240bf43c29be1bd82d77e602a61c798e9de02e5f8bb7bb414cb814f43236", + "sha256:8c3abf7eab5b76ae162c4fbb16d514a947fc57fd995b64e5ea8ef8ba3b888a69", + "sha256:93332c6972e4c91522c4810e907f3aea067424338071161b39cacded022559df", + "sha256:97d6c676dc97d593625d9fc48154f2ffeabb619a1e6fe8d2a5b53f97e3e9bdee", + "sha256:99dd85f0ca1db8d17a01a25c2bbb7784d25a2d39497c6beddbe96bff74194e04", + "sha256:9c7fb691fb07ec7ab99fd173bb0e7e0248d31bf83d484a87b917a342f63812c9", + "sha256:b3bc3cf200aab74f3d758586ac50295214eda496ac6a6636e0c881c5958d9123", + "sha256:bba54f97578943f48f621b4a7afb8eb022370da26a88b88ccc9fee9f3ef7ce45", + "sha256:bd2a13a0f8367e50347cbac87ae230ae1953935443240238f956bf10668bead6", + "sha256:cbc1184349ca6e5112898aa7fc3efa1b1bbae24ab1edc774cfd09cbfd3b091d7", + "sha256:cd82cca9c489e441574804dbda2dd8e114cf3be7935b03de11dade2c9478aea6", + "sha256:ce8ba5ed8b0a7a203922d61cff45ee6001a41a9359f04f00d055a4e988755569", + "sha256:cfee22e072a382b92ee0709dbb8203dabd52d54258051e770d9d2a81b162530b", + "sha256:d977df6f7c4109ed1d96ffb6795f6af77114be606ae4556efbfc9cac725db65d", + "sha256:da72a384a1d7e87490ca71182f3ab469ed21d847adc16b70c34faac5a3b12801", + "sha256:ddf4ad1d651e6c9234945061e1a31fe27a4be0dea21c498b87b186fadf8f5919", + "sha256:eb0ae5dfda83bbce660179d7b41c1c38fd833a54d2e6d9b258c644f3b75ef94d", + "sha256:f4c7d370badc60ac94a554bc571a46d03e39d8aacfba8006b334512e184aed59", + "sha256:f6c378b435a26fda8996579c0e324b108d2ca0d01b4661503a75634e5155559f", + "sha256:f6c9d30888503f2f5f87d6d41f016301352dd98da4a861bd10663c3a2d99d3b5", + "sha256:fab8a7877275060f7b303e1f91c218069a2814a616b6a5ee2d8a3737deb15915", + "sha256:fc32e7d7f98cac3d8d5153ed2cb583158ae3d446a6efb8e28ccb1c54a09f4169" + ], + "index": "pypi", + "version": "==23.1.0" }, "requests": { "hashes": [ - "sha256:68d7c56fd5a8999887728ef304a6d12edc7be74f1cfa47714fc8b414525c9a61", - "sha256:f22fa1e554c9ddfd16e6e41ac79759e17be9e492b3587efa038054674760e72d" + "sha256:bc7861137fbce630f17b03d3ad02ad0bf978c844f3536d0edda6499dafce2b6f", + "sha256:d568723a7ebd25875d8d1eaf5dfa068cd2fc8194b2e483d7b1f7c81918dbec6b" ], "index": "pypi", - "version": "==2.27.1" + "version": "==2.28.0" }, "scons": { "hashes": [ @@ -1118,11 +1155,11 @@ }, "setuptools": { "hashes": [ - "sha256:68e45d17c9281ba25dc0104eadd2647172b3472d9e01f911efa57965e8d51a36", - "sha256:a43bdedf853c670e5fed28e5623403bad2f73cf02f9a2774e91def6bda8265a7" + "sha256:d1746e7fd520e83bbe210d02fff1aa1a425ad671c7a9da7d246ec2401a087198", + "sha256:e7d11f3db616cda0751372244c2ba798e8e56a28e096ec4529010b803485f3fe" ], "markers": "python_version >= '3.7'", - "version": "==62.3.2" + "version": "==62.3.3" }, "six": { "hashes": [ @@ -1134,11 +1171,11 @@ }, "smbus2": { "hashes": [ - "sha256:6276eb599b76c4e74372f2582d2282f03b4398f0da16bc996608e4f21557ca9b", - "sha256:8b1e70cda011b6fb3caf8377a1084f73a5aa99392b78529f140b0a3f06468f6c" + "sha256:50f3c78e436b42a9583948be06961a8104cf020ebad5edfaaf2657528bef0818", + "sha256:634541ed794068a822fe7499f1577468b9d4641b68dd9bfb6d0eb7270f4d2a32" ], "index": "pypi", - "version": "==0.4.1" + "version": "==0.4.2" }, "sympy": { "hashes": [ @@ -1150,11 +1187,11 @@ }, "timezonefinder": { "hashes": [ - "sha256:90228f7bcf60388868ac95d3d6a8c9f710c766990909cb89dc39a893da7132ad", - "sha256:ce16831bc6349a82ca25ffc0e4c89df5bc5df29b286e5201701bce852f8e49f2" + "sha256:0471463083b5fef7a656c7e31a7376fa1941bf6a41a0750c6e0ca151e43646e7", + "sha256:2e8e958814f21fa809b16e98bd2f99082b69a0f88b864a4a1f2944d5a7e61827" ], "index": "pypi", - "version": "==6.0.0" + "version": "==6.0.1" }, "tomli": { "hashes": [ @@ -1164,6 +1201,14 @@ "markers": "python_version < '3.11'", "version": "==2.0.1" }, + "tomlkit": { + "hashes": [ + "sha256:0f4050db66fd445b885778900ce4dd9aea8c90c4721141fde0d6ade893820ef1", + "sha256:71ceb10c0eefd8b8f11fe34e8a51ad07812cb1dc3de23247425fbc9ddc47b9dd" + ], + "markers": "python_version >= '3.6' and python_version < '4'", + "version": "==0.11.0" + }, "tqdm": { "hashes": [ "sha256:40be55d30e200777a307a7585aee69e4eabb46b4ec6a4b4a5f2d9f11e7d5408d", @@ -1496,62 +1541,62 @@ "sha256:2857e29ff0d34db842cd7ca3230549d1a697f96ee6d3fb071cfa6c7393832597", "sha256:6881edbebdb17b39b4eaaa821b438bf6eddffb4468cf344f09f89def34a8b1df" ], - "markers": "python_version >= '3'", + "markers": "python_version >= '3.5'", "version": "==2.0.12" }, "control": { "hashes": [ - "sha256:8c9084bf386eafcf5d74008f780fae6dec68d243d18a380c866ac10a3549f8d3" + "sha256:0891d2d32d6006ac1faa4e238ed8223ca342a4721d202dfeccae24fb02563183" ], "index": "pypi", - "version": "==0.9.1" + "version": "==0.9.2" }, "coverage": { "hashes": [ - "sha256:00c8544510f3c98476bbd58201ac2b150ffbcce46a8c3e4fb89ebf01998f806a", - "sha256:016d7f5cf1c8c84f533a3c1f8f36126fbe00b2ec0ccca47cc5731c3723d327c6", - "sha256:03014a74023abaf5a591eeeaf1ac66a73d54eba178ff4cb1fa0c0a44aae70383", - "sha256:033ebec282793bd9eb988d0271c211e58442c31077976c19c442e24d827d356f", - "sha256:21e6686a95025927775ac501e74f5940cdf6fe052292f3a3f7349b0abae6d00f", - "sha256:26f8f92699756cb7af2b30720de0c5bb8d028e923a95b6d0c891088025a1ac8f", - "sha256:2e76bd16f0e31bc2b07e0fb1379551fcd40daf8cdf7e24f31a29e442878a827c", - "sha256:341e9c2008c481c5c72d0e0dbf64980a4b2238631a7f9780b0fe2e95755fb018", - "sha256:3cfd07c5889ddb96a401449109a8b97a165be9d67077df6802f59708bfb07720", - "sha256:4002f9e8c1f286e986fe96ec58742b93484195defc01d5cc7809b8f7acb5ece3", - "sha256:50ed480b798febce113709846b11f5d5ed1e529c88d8ae92f707806c50297abf", - "sha256:543e172ce4c0de533fa892034cce260467b213c0ea8e39da2f65f9a477425211", - "sha256:5a78cf2c43b13aa6b56003707c5203f28585944c277c1f3f109c7b041b16bd39", - "sha256:5cd698341626f3c77784858427bad0cdd54a713115b423d22ac83a28303d1d95", - "sha256:60c2147921da7f4d2d04f570e1838db32b95c5509d248f3fe6417e91437eaf41", - "sha256:62d382f7d77eeeaff14b30516b17bcbe80f645f5cf02bb755baac376591c653c", - "sha256:69432946f154c6add0e9ede03cc43b96e2ef2733110a77444823c053b1ff5166", - "sha256:727dafd7f67a6e1cad808dc884bd9c5a2f6ef1f8f6d2f22b37b96cb0080d4f49", - "sha256:742fb8b43835078dd7496c3c25a1ec8d15351df49fb0037bffb4754291ef30ce", - "sha256:750e13834b597eeb8ae6e72aa58d1d831b96beec5ad1d04479ae3772373a8088", - "sha256:7b546cf2b1974ddc2cb222a109b37c6ed1778b9be7e6b0c0bc0cf0438d9e45a6", - "sha256:83bd142cdec5e4a5c4ca1d4ff6fa807d28460f9db919f9f6a31babaaa8b88426", - "sha256:8d2e80dd3438e93b19e1223a9850fa65425e77f2607a364b6fd134fcd52dc9df", - "sha256:9229d074e097f21dfe0643d9d0140ee7433814b3f0fc3706b4abffd1e3038632", - "sha256:968ed5407f9460bd5a591cefd1388cc00a8f5099de9e76234655ae48cfdbe2c3", - "sha256:9c82f2cd69c71698152e943f4a5a6b83a3ab1db73b88f6e769fabc86074c3b08", - "sha256:a00441f5ea4504f5abbc047589d09e0dc33eb447dc45a1a527c8b74bfdd32c65", - "sha256:a022394996419142b33a0cf7274cb444c01d2bb123727c4bb0b9acabcb515dea", - "sha256:af5b9ee0fc146e907aa0f5fb858c3b3da9199d78b7bb2c9973d95550bd40f701", - "sha256:b5578efe4038be02d76c344007b13119b2b20acd009a88dde8adec2de4f630b5", - "sha256:b84ab65444dcc68d761e95d4d70f3cfd347ceca5a029f2ffec37d4f124f61311", - "sha256:c53ad261dfc8695062fc8811ac7c162bd6096a05a19f26097f411bdf5747aee7", - "sha256:cc173f1ce9ffb16b299f51c9ce53f66a62f4d975abe5640e976904066f3c835d", - "sha256:d548edacbf16a8276af13063a2b0669d58bbcfca7c55a255f84aac2870786a61", - "sha256:d55fae115ef9f67934e9f1103c9ba826b4c690e4c5bcf94482b8b2398311bf9c", - "sha256:d8099ea680201c2221f8468c372198ceba9338a5fec0e940111962b03b3f716a", - "sha256:e35217031e4b534b09f9b9a5841b9344a30a6357627761d4218818b865d45055", - "sha256:e4f52c272fdc82e7c65ff3f17a7179bc5f710ebc8ce8a5cadac81215e8326740", - "sha256:e637ae0b7b481905358624ef2e81d7fb0b1af55f5ff99f9ba05442a444b11e45", - "sha256:eef5292b60b6de753d6e7f2d128d5841c7915fb1e3321c3a1fe6acfe76c38052", - "sha256:fb45fe08e1abc64eb836d187b20a59172053999823f7f6ef4f18a819c44ba16f" - ], - "index": "pypi", - "version": "==6.4" + "sha256:01c5615d13f3dd3aa8543afc069e5319cfa0c7d712f6e04b920431e5c564a749", + "sha256:106c16dfe494de3193ec55cac9640dd039b66e196e4641fa8ac396181578b982", + "sha256:129cd05ba6f0d08a766d942a9ed4b29283aff7b2cccf5b7ce279d50796860bb3", + "sha256:145f296d00441ca703a659e8f3eb48ae39fb083baba2d7ce4482fb2723e050d9", + "sha256:1480ff858b4113db2718848d7b2d1b75bc79895a9c22e76a221b9d8d62496428", + "sha256:269eaa2c20a13a5bf17558d4dc91a8d078c4fa1872f25303dddcbba3a813085e", + "sha256:26dff09fb0d82693ba9e6231248641d60ba606150d02ed45110f9ec26404ed1c", + "sha256:2bd9a6fc18aab8d2e18f89b7ff91c0f34ff4d5e0ba0b33e989b3cd4194c81fd9", + "sha256:309ce4a522ed5fca432af4ebe0f32b21d6d7ccbb0f5fcc99290e71feba67c264", + "sha256:3384f2a3652cef289e38100f2d037956194a837221edd520a7ee5b42d00cc605", + "sha256:342d4aefd1c3e7f620a13f4fe563154d808b69cccef415415aece4c786665397", + "sha256:39ee53946bf009788108b4dd2894bf1349b4e0ca18c2016ffa7d26ce46b8f10d", + "sha256:4321f075095a096e70aff1d002030ee612b65a205a0a0f5b815280d5dc58100c", + "sha256:4803e7ccf93230accb928f3a68f00ffa80a88213af98ed338a57ad021ef06815", + "sha256:4ce1b258493cbf8aec43e9b50d89982346b98e9ffdfaae8ae5793bc112fb0068", + "sha256:664a47ce62fe4bef9e2d2c430306e1428ecea207ffd68649e3b942fa8ea83b0b", + "sha256:75ab269400706fab15981fd4bd5080c56bd5cc07c3bccb86aab5e1d5a88dc8f4", + "sha256:83c4e737f60c6936460c5be330d296dd5b48b3963f48634c53b3f7deb0f34ec4", + "sha256:84631e81dd053e8a0d4967cedab6db94345f1c36107c71698f746cb2636c63e3", + "sha256:84e65ef149028516c6d64461b95a8dbcfce95cfd5b9eb634320596173332ea84", + "sha256:865d69ae811a392f4d06bde506d531f6a28a00af36f5c8649684a9e5e4a85c83", + "sha256:87f4f3df85aa39da00fd3ec4b5abeb7407e82b68c7c5ad181308b0e2526da5d4", + "sha256:8c08da0bd238f2970230c2a0d28ff0e99961598cb2e810245d7fc5afcf1254e8", + "sha256:961e2fb0680b4f5ad63234e0bf55dfb90d302740ae9c7ed0120677a94a1590cb", + "sha256:9b3e07152b4563722be523e8cd0b209e0d1a373022cfbde395ebb6575bf6790d", + "sha256:a7f3049243783df2e6cc6deafc49ea123522b59f464831476d3d1448e30d72df", + "sha256:bf5601c33213d3cb19d17a796f8a14a9eaa5e87629a53979a5981e3e3ae166f6", + "sha256:cec3a0f75c8f1031825e19cd86ee787e87cf03e4fd2865c79c057092e69e3a3b", + "sha256:d42c549a8f41dc103a8004b9f0c433e2086add8a719da00e246e17cbe4056f72", + "sha256:d67d44996140af8b84284e5e7d398e589574b376fb4de8ccd28d82ad8e3bea13", + "sha256:d9c80df769f5ec05ad21ea34be7458d1dc51ff1fb4b2219e77fe24edf462d6df", + "sha256:e57816f8ffe46b1df8f12e1b348f06d164fd5219beba7d9433ba79608ef011cc", + "sha256:ee2ddcac99b2d2aec413e36d7a429ae9ebcadf912946b13ffa88e7d4c9b712d6", + "sha256:f02cbbf8119db68455b9d763f2f8737bb7db7e43720afa07d8eb1604e5c5ae28", + "sha256:f1d5aa2703e1dab4ae6cf416eb0095304f49d004c39e9db1d86f57924f43006b", + "sha256:f5b66caa62922531059bc5ac04f836860412f7f88d38a476eda0a6f11d4724f4", + "sha256:f69718750eaae75efe506406c490d6fc5a6161d047206cc63ce25527e8a3adad", + "sha256:fb73e0011b8793c053bfa85e53129ba5f0250fdc0392c1591fd35d915ec75c46", + "sha256:fd180ed867e289964404051a958f7cccabdeed423f91a899829264bb7974d3d3", + "sha256:fdb6f7bd51c2d1714cea40718f6149ad9be6a2ee7d93b19e9f00934c0f2a74d9", + "sha256:ffa9297c3a453fba4717d06df579af42ab9a28022444cae7fa605af4df612d54" + ], + "index": "pypi", + "version": "==6.4.1" }, "cryptography": { "hashes": [ @@ -1665,11 +1710,11 @@ }, "filelock": { "hashes": [ - "sha256:b795f1b42a61bbf8ec7113c341dad679d772567b936fbd1bf43c9a238e673e20", - "sha256:c7b5fdb219b398a5b28c8e4c1893ef5f98ece6a38c6ab2c22e26ec161556fed6" + "sha256:37def7b658813cda163b56fc564cdc75e86d338246458c4c28ae84cabefa2404", + "sha256:3a0fd85166ad9dbab54c9aec96737b744106dc5f15c0b09a6744a445299fcf04" ], "markers": "python_version >= '3.7'", - "version": "==3.7.0" + "version": "==3.7.1" }, "fonttools": { "hashes": [ @@ -1688,11 +1733,11 @@ }, "hypothesis": { "hashes": [ - "sha256:2696cdb9005946bf1d2b215cc91d3fc01625e3342eb8743ddd04b667b2f1882b", - "sha256:967009fa561b3a3f8363a73d71923357271c37dc7fa27b30c2d21a1b6092b240" + "sha256:4ad26c5d434171ffc02aba569dd52255573d615554c062bc30734dbe6f318c61", + "sha256:69978811f1d9c19710c7d2bf8233dc43c80efa964251b72efbe8274044e073b4" ], "index": "pypi", - "version": "==6.46.7" + "version": "==6.47.1" }, "identify": { "hashes": [ @@ -1707,7 +1752,7 @@ "sha256:84d9dd047ffa80596e0f246e2eab0b391788b0503584e8945f2368256d2735ff", "sha256:9d643ff0a55b762d5cdb124b8eaa99c66322e2157b69160bc32796e824360e6d" ], - "markers": "python_version >= '3'", + "markers": "python_version >= '3.5'", "version": "==3.3" }, "imagesize": { @@ -1918,41 +1963,40 @@ }, "mpld3": { "hashes": [ - "sha256:3626d37da7ac11d0fce4dd5ceb37d04ba618bf951129bf6ca3f94bf48da87d6f", - "sha256:b1290f6cca2d9515e32ee21a8b0c18b2ea361cefce58ba0f4df881e9eeb3f64c", - "sha256:c589db8b661aee25c93e198e2e18ed47b9a96951de41d96241345acec5f819ab" + "sha256:1a167dbef836dd7c66d8aa71c06a32d50bffa18725f304d93cb74fdb3545043b", + "sha256:41938e65de4ba41a1b53d92e7c8609e7172e09b33ef5db42bb6f73701106c8b7" ], "index": "pypi", - "version": "==0.5.7" + "version": "==0.5.8" }, "mypy": { "hashes": [ - "sha256:0112752a6ff07230f9ec2f71b0d3d4e088a910fdce454fdb6553e83ed0eced7d", - "sha256:0384d9f3af49837baa92f559d3fa673e6d2652a16550a9ee07fc08c736f5e6f8", - "sha256:1b333cfbca1762ff15808a0ef4f71b5d3eed8528b23ea1c3fb50543c867d68de", - "sha256:1fdeb0a0f64f2a874a4c1f5271f06e40e1e9779bf55f9567f149466fc7a55038", - "sha256:4c653e4846f287051599ed8f4b3c044b80e540e88feec76b11044ddc5612ffed", - "sha256:563514c7dc504698fb66bb1cf897657a173a496406f1866afae73ab5b3cdb334", - "sha256:5b231afd6a6e951381b9ef09a1223b1feabe13625388db48a8690f8daa9b71ff", - "sha256:5ce6a09042b6da16d773d2110e44f169683d8cc8687e79ec6d1181a72cb028d2", - "sha256:5e7647df0f8fc947388e6251d728189cfadb3b1e558407f93254e35abc026e22", - "sha256:6003de687c13196e8a1243a5e4bcce617d79b88f83ee6625437e335d89dfebe2", - "sha256:61504b9a5ae166ba5ecfed9e93357fd51aa693d3d434b582a925338a2ff57fd2", - "sha256:77423570c04aca807508a492037abbd72b12a1fb25a385847d191cd50b2c9605", - "sha256:a4d9898f46446bfb6405383b57b96737dcfd0a7f25b748e78ef3e8c576bba3cb", - "sha256:a952b8bc0ae278fc6316e6384f67bb9a396eb30aced6ad034d3a76120ebcc519", - "sha256:b5b5bd0ffb11b4aba2bb6d31b8643902c48f990cc92fda4e21afac658044f0c0", - "sha256:ca75ecf2783395ca3016a5e455cb322ba26b6d33b4b413fcdedfc632e67941dc", - "sha256:cf9c261958a769a3bd38c3e133801ebcd284ffb734ea12d01457cb09eacf7d7b", - "sha256:dd4d670eee9610bf61c25c940e9ade2d0ed05eb44227275cce88701fee014b1f", - "sha256:e19736af56947addedce4674c0971e5dceef1b5ec7d667fe86bcd2b07f8f9075", - "sha256:eaea21d150fb26d7b4856766e7addcf929119dd19fc832b22e71d942835201ef", - "sha256:eaff8156016487c1af5ffa5304c3e3fd183edcb412f3e9c72db349faf3f6e0eb", - "sha256:ee0a36edd332ed2c5208565ae6e3a7afc0eabb53f5327e281f2ef03a6bc7687a", - "sha256:ef7beb2a3582eb7a9f37beaf38a28acfd801988cde688760aea9e6cc4832b10b" - ], - "index": "pypi", - "version": "==0.950" + "sha256:006be38474216b833eca29ff6b73e143386f352e10e9c2fbe76aa8549e5554f5", + "sha256:03c6cc893e7563e7b2949b969e63f02c000b32502a1b4d1314cabe391aa87d66", + "sha256:0e9f70df36405c25cc530a86eeda1e0867863d9471fe76d1273c783df3d35c2e", + "sha256:1ece702f29270ec6af25db8cf6185c04c02311c6bb21a69f423d40e527b75c56", + "sha256:3e09f1f983a71d0672bbc97ae33ee3709d10c779beb613febc36805a6e28bb4e", + "sha256:439c726a3b3da7ca84a0199a8ab444cd8896d95012c4a6c4a0d808e3147abf5d", + "sha256:5a0b53747f713f490affdceef835d8f0cb7285187a6a44c33821b6d1f46ed813", + "sha256:5f1332964963d4832a94bebc10f13d3279be3ce8f6c64da563d6ee6e2eeda932", + "sha256:63e85a03770ebf403291ec50097954cc5caf2a9205c888ce3a61bd3f82e17569", + "sha256:64759a273d590040a592e0f4186539858c948302c653c2eac840c7a3cd29e51b", + "sha256:697540876638ce349b01b6786bc6094ccdaba88af446a9abb967293ce6eaa2b0", + "sha256:9940e6916ed9371809b35b2154baf1f684acba935cd09928952310fbddaba648", + "sha256:9f5f5a74085d9a81a1f9c78081d60a0040c3efb3f28e5c9912b900adf59a16e6", + "sha256:a5ea0875a049de1b63b972456542f04643daf320d27dc592d7c3d9cd5d9bf950", + "sha256:b117650592e1782819829605a193360a08aa99f1fc23d1d71e1a75a142dc7e15", + "sha256:b24be97351084b11582fef18d79004b3e4db572219deee0212078f7cf6352723", + "sha256:b88f784e9e35dcaa075519096dc947a388319cb86811b6af621e3523980f1c8a", + "sha256:bdd5ca340beffb8c44cb9dc26697628d1b88c6bddf5c2f6eb308c46f269bb6f3", + "sha256:d5aaf1edaa7692490f72bdb9fbd941fbf2e201713523bdb3f4038be0af8846c6", + "sha256:e999229b9f3198c0c880d5e269f9f8129c8862451ce53a011326cad38b9ccd24", + "sha256:f4a21d01fc0ba4e31d82f0fff195682e29f9401a8bdb7173891070eb260aeb3b", + "sha256:f4b794db44168a4fc886e3450201365c9526a522c46ba089b55e1f11c163750d", + "sha256:f730d56cb924d371c26b8eaddeea3cc07d78ff51c521c6d04899ac6904b75492" + ], + "index": "pypi", + "version": "==0.961" }, "mypy-extensions": { "hashes": [ @@ -1963,11 +2007,11 @@ }, "myst-parser": { "hashes": [ - "sha256:1635ce3c18965a528d6de980f989ff64d6a1effb482e1f611b1bfb79e38f3d98", - "sha256:4c076d649e066f9f5c7c661bae2658be1ca06e76b002bb97f02a09398707686c" + "sha256:4965e51918837c13bf1c6f6fe2c6bddddf193148360fbdaefe743a4981358f6a", + "sha256:739a4d96773a8e55a2cacd3941ce46a446ee23dcd6b37e06f73f551ad7821d86" ], "index": "pypi", - "version": "==0.17.2" + "version": "==0.18.0" }, "natsort": { "hashes": [ @@ -2014,16 +2058,16 @@ }, "opencv-python-headless": { "hashes": [ - "sha256:3f330468c29882dbbec5af25695c5e575572c6b855cb0f9fe53e14116fd46bfc", - "sha256:4bdf982574bf2fefc5f82c86df7cb42e56ad627874c7c0f4d94ecf4ae8885304", - "sha256:567a54c1919bcf5b3d20a9830e3c511e57134de8def286ce137c3544a892f98c", - "sha256:62e31878641a8f96e773118d1eea9f34bdda87c9990a0faab04ebaafb5ae015c", - "sha256:a60e9ff48854ec37be391e19dd634883cc26c2f0f814e5325b3deca33420912c", - "sha256:c3c2dda44d601757a508b07d628537c49f31223ad4edd0f747a70d4c852a7b98", - "sha256:ca4f013fa958f60fb2327fe87e6127c1ac0ab536890b1d4b00847f417b7af1ba" + "sha256:21e70f8b0c04098cdf466d27184fe6c3820aaef944a22548db95099959c95889", + "sha256:2c032c373e447c3fc2a670bca20e2918a1205a6e72854df60425fd3f82c78c32", + "sha256:3bacd806cce1f1988e58f3d6f761538e0215d6621d316de94c009dc0acbd6ad3", + "sha256:d5291d7e10aa2c19cab6fd86f0d61af8617290ecd2d7ffcb051e446868d04cc5", + "sha256:e6c069bc963d7e8fcec21b3e33e594d35948badd63eccb2e80f88b0a12102c03", + "sha256:eec6281054346103d6af93f173b7c6a206beb2663d3adc04aa3ddc66e85093df", + "sha256:ffbf26fcd697af996408440a93bc69c49c05a36845771f984156dfbeaa95d497" ], "index": "pypi", - "version": "==4.5.5.64" + "version": "==4.6.0.66" }, "packaging": { "hashes": [ @@ -2329,11 +2373,11 @@ }, "requests": { "hashes": [ - "sha256:68d7c56fd5a8999887728ef304a6d12edc7be74f1cfa47714fc8b414525c9a61", - "sha256:f22fa1e554c9ddfd16e6e41ac79759e17be9e492b3587efa038054674760e72d" + "sha256:bc7861137fbce630f17b03d3ad02ad0bf978c844f3536d0edda6499dafce2b6f", + "sha256:d568723a7ebd25875d8d1eaf5dfa068cd2fc8194b2e483d7b1f7c81918dbec6b" ], "index": "pypi", - "version": "==2.27.1" + "version": "==2.28.0" }, "reverse-geocoder": { "hashes": [ From 66bc246210b4471e0a5fa3ab792c4d9fe0316406 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 10 Jun 2022 15:23:42 -0700 Subject: [PATCH 32/93] count events: add simple camera debugging --- selfdrive/debug/count_events.py | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/selfdrive/debug/count_events.py b/selfdrive/debug/count_events.py index 8b32ce9d21..c3870e0f9e 100755 --- a/selfdrive/debug/count_events.py +++ b/selfdrive/debug/count_events.py @@ -4,6 +4,7 @@ from collections import Counter from pprint import pprint from tqdm import tqdm +from cereal.services import service_list from tools.lib.route import Route from tools.lib.logreader import LogReader @@ -13,6 +14,9 @@ if __name__ == "__main__": cnt_valid: Counter = Counter() cnt_events: Counter = Counter() + cams = [s for s in service_list if s.endswith('CameraState')] + cnt_cameras = dict.fromkeys(cams, 0) + for q in tqdm(r.qlog_paths()): if q is None: continue @@ -21,12 +25,21 @@ if __name__ == "__main__": if msg.which() == 'carEvents': for e in msg.carEvents: cnt_events[e.name] += 1 + elif msg.which() in cams: + cnt_cameras[msg.which()] += 1 + if not msg.valid: cnt_valid[msg.which()] += 1 + print("Events") pprint(cnt_events) - print("\n\n") + print("\n") print("Not valid") pprint(cnt_valid) + + print("\n") + print("Cameras") + for k, v in cnt_cameras.items(): + print(" ", k.ljust(20), v) From c646eeee0ac54925db5afc51b95c5d869d6dba68 Mon Sep 17 00:00:00 2001 From: ZwX1616 Date: Fri, 10 Jun 2022 16:16:46 -0700 Subject: [PATCH 33/93] Revert fullframe DM model (#24812) * Revert "fullframe DM: flip RHD yaw to use matching thresholds" This reverts commit 2ac693100359d9df34d5b2b450a0058f2dd6b7c4. * Revert "fullframe DM model (#24762)" This reverts commit d6c07a6b158595c1ab10256dc6eee4fbdf902379. * revert cereal --- cereal | 2 +- common/modeldata.h | 6 + selfdrive/camerad/cameras/camera_common.cc | 2 +- selfdrive/camerad/cameras/camera_qcom2.cc | 2 +- selfdrive/hardware/tici/test_power_draw.py | 2 +- selfdrive/modeld/dmonitoringmodeld.cc | 6 +- selfdrive/modeld/models/dmonitoring.cc | 234 ++++++++++++------ selfdrive/modeld/models/dmonitoring.h | 33 +-- .../modeld/models/dmonitoring_model.current | 4 +- .../modeld/models/dmonitoring_model.onnx | 4 +- .../modeld/models/dmonitoring_model_q.dlc | 4 +- selfdrive/modeld/runners/onnx_runner.py | 21 +- selfdrive/modeld/runners/onnxmodel.cc | 6 +- selfdrive/modeld/runners/onnxmodel.h | 3 +- selfdrive/modeld/runners/snpemodel.cc | 12 +- selfdrive/modeld/runners/snpemodel.h | 3 +- selfdrive/monitoring/dmonitoringd.py | 7 +- selfdrive/monitoring/driver_monitor.py | 90 ++++--- selfdrive/monitoring/test_monitoring.py | 24 +- selfdrive/test/process_replay/model_replay.py | 8 +- .../process_replay/model_replay_ref_commit | 2 +- .../test/process_replay/process_replay.py | 2 +- selfdrive/test/test_onroad.py | 6 +- selfdrive/ui/qt/offroad/driverview.cc | 43 ++-- selfdrive/ui/qt/widgets/cameraview.cc | 9 +- 25 files changed, 308 insertions(+), 227 deletions(-) diff --git a/cereal b/cereal index 96cbed052a..e9d3597d23 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 96cbed052ab1d69ea15da94dc2b882d59a786347 +Subproject commit e9d3597d23311a3e33a2def74ceec839e5ff4bf5 diff --git a/common/modeldata.h b/common/modeldata.h index 410a69ea65..06d9398031 100644 --- a/common/modeldata.h +++ b/common/modeldata.h @@ -24,6 +24,12 @@ 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); +namespace tici_dm_crop { + const int x_offset = -72; + const int y_offset = -144; + const int width = 954; +}; + const mat3 fcam_intrinsic_matrix = (mat3){{2648.0, 0.0, 1928.0 / 2, 0.0, 2648.0, 1208.0 / 2, 0.0, 0.0, 1.0}}; diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc index 21e225d538..049324f085 100644 --- a/selfdrive/camerad/cameras/camera_common.cc +++ b/selfdrive/camerad/cameras/camera_common.cc @@ -239,7 +239,7 @@ static kj::Array yuv420_to_jpeg(const CameraBuf *b, int thumbnail_w 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]); + 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; diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc index b949dfb80f..d43beb0921 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.cc +++ b/selfdrive/camerad/cameras/camera_qcom2.cc @@ -837,7 +837,7 @@ void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_i s->road_cam.camera_init(s, v, CAMERA_ID_AR0231, 1, 20, device_id, ctx, VISION_STREAM_RGB_ROAD, VISION_STREAM_ROAD, !env_disable_road); s->wide_road_cam.camera_init(s, v, CAMERA_ID_AR0231, 0, 20, device_id, ctx, VISION_STREAM_RGB_WIDE_ROAD, VISION_STREAM_WIDE_ROAD, !env_disable_wide_road); - s->sm = new SubMaster({"driverStateV2"}); + s->sm = new SubMaster({"driverState"}); s->pm = new PubMaster({"roadCameraState", "driverCameraState", "wideRoadCameraState", "thumbnail"}); } diff --git a/selfdrive/hardware/tici/test_power_draw.py b/selfdrive/hardware/tici/test_power_draw.py index 5eb84514e5..ab2d691a09 100755 --- a/selfdrive/hardware/tici/test_power_draw.py +++ b/selfdrive/hardware/tici/test_power_draw.py @@ -21,7 +21,7 @@ class Proc: PROCS = [ Proc('camerad', 2.15), Proc('modeld', 1.0), - Proc('dmonitoringmodeld', 0.35), + Proc('dmonitoringmodeld', 0.25), Proc('encoderd', 0.23), ] diff --git a/selfdrive/modeld/dmonitoringmodeld.cc b/selfdrive/modeld/dmonitoringmodeld.cc index cde13a9bee..68c49572e6 100644 --- a/selfdrive/modeld/dmonitoringmodeld.cc +++ b/selfdrive/modeld/dmonitoringmodeld.cc @@ -12,7 +12,7 @@ ExitHandler do_exit; void run_model(DMonitoringModelState &model, VisionIpcClient &vipc_client) { - PubMaster pm({"driverStateV2"}); + PubMaster pm({"driverState"}); SubMaster sm({"liveCalibration"}); float calib[CALIB_LEN] = {0}; double last = 0; @@ -31,11 +31,11 @@ void run_model(DMonitoringModelState &model, VisionIpcClient &vipc_client) { } double t1 = millis_since_boot(); - DMonitoringModelResult model_res = dmonitoring_eval_frame(&model, buf->addr, buf->width, buf->height, buf->stride, buf->uv_offset, 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 - dmonitoring_publish(pm, extra.frame_id, model_res, (t2 - t1) / 1000.0, model.output); + dmonitoring_publish(pm, extra.frame_id, res, (t2 - t1) / 1000.0, model.output); //printf("dmonitoring process: %.2fms, from last %.2fms\n", t2 - t1, t1 - last); last = t1; diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc index 20294c3f3c..e134ad3a5a 100644 --- a/selfdrive/modeld/models/dmonitoring.cc +++ b/selfdrive/modeld/models/dmonitoring.cc @@ -10,8 +10,8 @@ #include "selfdrive/modeld/models/dmonitoring.h" -constexpr int MODEL_WIDTH = 1440; -constexpr int MODEL_HEIGHT = 960; +constexpr int MODEL_WIDTH = 320; +constexpr int MODEL_HEIGHT = 640; template static inline T *get_buffer(std::vector &buf, const size_t size) { @@ -19,115 +19,199 @@ static inline T *get_buffer(std::vector &buf, const size_t size) { return buf.data(); } +static inline void init_yuv_buf(std::vector &buf, const int width, int height) { + uint8_t *y = get_buffer(buf, width * height * 3 / 2); + uint8_t *u = y + width * height; + uint8_t *v = u + (width / 2) * (height / 2); + + // needed on comma two to make the padded border black + // equivalent to RGB(0,0,0) in YUV space + memset(y, 16, width * height); + memset(u, 128, (width / 2) * (height / 2)); + memset(v, 128, (width / 2) * (height / 2)); +} + void dmonitoring_init(DMonitoringModelState* s) { s->is_rhd = Params().getBool("IsRHD"); + for (int x = 0; x < std::size(s->tensor); ++x) { + s->tensor[x] = (x - 128.f) * 0.0078125f; + } + init_yuv_buf(s->resized_buf, MODEL_WIDTH, MODEL_HEIGHT); #ifdef USE_ONNX_MODEL - s->m = new ONNXModel("models/dmonitoring_model.onnx", &s->output[0], OUTPUT_SIZE, USE_DSP_RUNTIME, false, true); + s->m = new ONNXModel("models/dmonitoring_model.onnx", &s->output[0], OUTPUT_SIZE, USE_DSP_RUNTIME); #else - s->m = new SNPEModel("models/dmonitoring_model_q.dlc", &s->output[0], OUTPUT_SIZE, USE_DSP_RUNTIME, false, true); + s->m = new SNPEModel("models/dmonitoring_model_q.dlc", &s->output[0], OUTPUT_SIZE, USE_DSP_RUNTIME); #endif s->m->addCalib(s->calib, CALIB_LEN); } -void parse_driver_data(DriverStateResult &ds_res, const DMonitoringModelState* s, int out_idx_offset) { - for (int i = 0; i < 3; ++i) { - ds_res.face_orientation[i] = s->output[out_idx_offset+i] * REG_SCALE; - ds_res.face_orientation_std[i] = exp(s->output[out_idx_offset+6+i]); - } - for (int i = 0; i < 2; ++i) { - ds_res.face_position[i] = s->output[out_idx_offset+3+i] * REG_SCALE; - ds_res.face_position_std[i] = exp(s->output[out_idx_offset+9+i]); - } - for (int i = 0; i < 4; ++i) { - ds_res.ready_prob[i] = sigmoid(s->output[out_idx_offset+35+i]); - } - for (int i = 0; i < 2; ++i) { - ds_res.not_ready_prob[i] = sigmoid(s->output[out_idx_offset+39+i]); - } - ds_res.face_prob = sigmoid(s->output[out_idx_offset+12]); - ds_res.left_eye_prob = sigmoid(s->output[out_idx_offset+21]); - ds_res.right_eye_prob = sigmoid(s->output[out_idx_offset+30]); - ds_res.left_blink_prob = sigmoid(s->output[out_idx_offset+31]); - ds_res.right_blink_prob = sigmoid(s->output[out_idx_offset+32]); - ds_res.sunglasses_prob = sigmoid(s->output[out_idx_offset+33]); - ds_res.occluded_prob = sigmoid(s->output[out_idx_offset+34]); +static inline auto get_yuv_buf(std::vector &buf, const int width, int height) { + uint8_t *y = get_buffer(buf, width * height * 3 / 2); + uint8_t *u = y + width * height; + uint8_t *v = u + (width /2) * (height / 2); + return std::make_tuple(y, u, v); } -void fill_driver_data(cereal::DriverStateV2::DriverData::Builder ddata, const DriverStateResult &ds_res) { - ddata.setFaceOrientation(ds_res.face_orientation); - ddata.setFaceOrientationStd(ds_res.face_orientation_std); - ddata.setFacePosition(ds_res.face_position); - ddata.setFacePositionStd(ds_res.face_position_std); - ddata.setFaceProb(ds_res.face_prob); - ddata.setLeftEyeProb(ds_res.left_eye_prob); - ddata.setRightEyeProb(ds_res.right_eye_prob); - ddata.setLeftBlinkProb(ds_res.left_blink_prob); - ddata.setRightBlinkProb(ds_res.right_blink_prob); - ddata.setSunglassesProb(ds_res.sunglasses_prob); - ddata.setOccludedProb(ds_res.occluded_prob); - ddata.setReadyProb(ds_res.ready_prob); - ddata.setNotReadyProb(ds_res.not_ready_prob); +struct Rect {int x, y, w, h;}; +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_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) * 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]; + } + } } -DMonitoringModelResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, int stride, int uv_offset, float *calib) { - int v_off = height - MODEL_HEIGHT; - int h_off = (width - MODEL_WIDTH) / 2; - int yuv_buf_len = MODEL_WIDTH * MODEL_HEIGHT; - - uint8_t *raw_buf = (uint8_t *) stream_buf; - // vertical crop free - uint8_t *raw_y_start = raw_buf + stride * v_off; +DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, int stride, int uv_offset, float *calib) { + 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; + } - uint8_t *net_input_buf = get_buffer(s->net_input_buf, yuv_buf_len); + int resized_width = MODEL_WIDTH; + int resized_height = MODEL_HEIGHT; + + auto [cropped_y, cropped_u, cropped_v] = get_yuv_buf(s->cropped_buf, crop_rect.w, crop_rect.h); + if (!s->is_rhd) { + 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_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, + cropped_y, crop_rect.w, + cropped_u, crop_rect.w / 2, + cropped_v, crop_rect.w / 2, + crop_rect.w, crop_rect.h); + } - // here makes a uint8 copy - for (int r = 0; r < MODEL_HEIGHT; ++r) { - memcpy(net_input_buf + r * MODEL_WIDTH, raw_y_start + r * stride + h_off, MODEL_WIDTH); + 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; + 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); + // one shot conversion, O(n) anyway + // yuvframe2tensor, normalize + for (int r = 0; r < MODEL_HEIGHT/2; r++) { + for (int c = 0; c < MODEL_WIDTH/2; c++) { + // Y_ul + net_input_buf[(r*MODEL_WIDTH/2) + c + (0*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = s->tensor[resized_y[(2*r)*resized_width + 2*c]]; + // Y_dl + net_input_buf[(r*MODEL_WIDTH/2) + c + (1*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = s->tensor[resized_y[(2*r+1)*resized_width + 2*c]]; + // Y_ur + net_input_buf[(r*MODEL_WIDTH/2) + c + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = s->tensor[resized_y[(2*r)*resized_width + 2*c+1]]; + // Y_dr + net_input_buf[(r*MODEL_WIDTH/2) + c + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = s->tensor[resized_y[(2*r+1)*resized_width + 2*c+1]]; + // U + net_input_buf[(r*MODEL_WIDTH/2) + c + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = s->tensor[resized_u[r*resized_width/2 + c]]; + // V + net_input_buf[(r*MODEL_WIDTH/2) + c + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = s->tensor[resized_v[r*resized_width/2 + c]]; + } } - // printf("preprocess completed. %d \n", yuv_buf_len); - // FILE *dump_yuv_file = fopen("/tmp/rawdump.yuv", "wb"); - // fwrite(net_input_buf, yuv_buf_len, sizeof(uint8_t), dump_yuv_file); - // fclose(dump_yuv_file); + //printf("preprocess completed. %d \n", yuv_buf_len); + //FILE *dump_yuv_file = fopen("/tmp/rawdump.yuv", "wb"); + //fwrite(resized_buf, yuv_buf_len, sizeof(uint8_t), dump_yuv_file); + //fclose(dump_yuv_file); + + // *** testing *** + // idat = np.frombuffer(open("/tmp/inputdump.yuv", "rb").read(), np.float32).reshape(6, 160, 320) + // imshow(cv2.cvtColor(tensor_to_frames(idat[None]/0.0078125+128)[0], cv2.COLOR_YUV2RGB_I420)) + + //FILE *dump_yuv_file2 = fopen("/tmp/inputdump.yuv", "wb"); + //fwrite(net_input_buf, MODEL_HEIGHT*MODEL_WIDTH*3/2, sizeof(float), dump_yuv_file2); + //fclose(dump_yuv_file2); double t1 = millis_since_boot(); - s->m->addImage((float*)net_input_buf, yuv_buf_len / 4); + s->m->addImage(net_input_buf, yuv_buf_len); for (int i = 0; i < CALIB_LEN; i++) { s->calib[i] = calib[i]; } s->m->execute(); double t2 = millis_since_boot(); - DMonitoringModelResult model_res = {0}; - parse_driver_data(model_res.driver_state_lhd, s, 0); - parse_driver_data(model_res.driver_state_rhd, s, 41); - model_res.poor_vision_prob = sigmoid(s->output[82]); - model_res.wheel_on_right_prob = sigmoid(s->output[83]); - model_res.dsp_execution_time = (t2 - t1) / 1000.; - - return model_res; + DMonitoringResult ret = {0}; + for (int i = 0; i < 3; ++i) { + ret.face_orientation[i] = s->output[i] * REG_SCALE; + ret.face_orientation_meta[i] = exp(s->output[6 + i]); + } + for (int i = 0; i < 2; ++i) { + ret.face_position[i] = s->output[3 + i] * REG_SCALE; + ret.face_position_meta[i] = exp(s->output[9 + i]); + } + for (int i = 0; i < 4; ++i) { + ret.ready_prob[i] = sigmoid(s->output[39 + i]); + } + for (int i = 0; i < 2; ++i) { + ret.not_ready_prob[i] = sigmoid(s->output[43 + i]); + } + ret.face_prob = sigmoid(s->output[12]); + ret.left_eye_prob = sigmoid(s->output[21]); + ret.right_eye_prob = sigmoid(s->output[30]); + ret.left_blink_prob = sigmoid(s->output[31]); + ret.right_blink_prob = sigmoid(s->output[32]); + ret.sg_prob = sigmoid(s->output[33]); + ret.poor_vision = sigmoid(s->output[34]); + ret.partial_face = sigmoid(s->output[35]); + ret.distracted_pose = sigmoid(s->output[36]); + ret.distracted_eyes = sigmoid(s->output[37]); + ret.occluded_prob = sigmoid(s->output[38]); + ret.dsp_execution_time = (t2 - t1) / 1000.; + return ret; } -void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringModelResult &model_res, float execution_time, kj::ArrayPtr raw_pred) { +void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, float execution_time, kj::ArrayPtr raw_pred) { // make msg MessageBuilder msg; - auto framed = msg.initEvent().initDriverStateV2(); + auto framed = msg.initEvent().initDriverState(); framed.setFrameId(frame_id); framed.setModelExecutionTime(execution_time); - framed.setDspExecutionTime(model_res.dsp_execution_time); - - framed.setPoorVisionProb(model_res.poor_vision_prob); - framed.setWheelOnRightProb(model_res.wheel_on_right_prob); - fill_driver_data(framed.initLeftDriverData(), model_res.driver_state_lhd); - fill_driver_data(framed.initRightDriverData(), model_res.driver_state_rhd); - + framed.setDspExecutionTime(res.dsp_execution_time); + + framed.setFaceOrientation(res.face_orientation); + framed.setFaceOrientationStd(res.face_orientation_meta); + framed.setFacePosition(res.face_position); + framed.setFacePositionStd(res.face_position_meta); + framed.setFaceProb(res.face_prob); + framed.setLeftEyeProb(res.left_eye_prob); + framed.setRightEyeProb(res.right_eye_prob); + framed.setLeftBlinkProb(res.left_blink_prob); + framed.setRightBlinkProb(res.right_blink_prob); + framed.setSunglassesProb(res.sg_prob); + framed.setPoorVision(res.poor_vision); + framed.setPartialFace(res.partial_face); + framed.setDistractedPose(res.distracted_pose); + framed.setDistractedEyes(res.distracted_eyes); + framed.setOccludedProb(res.occluded_prob); + framed.setReadyProb(res.ready_prob); + framed.setNotReadyProb(res.not_ready_prob); if (send_raw_pred) { framed.setRawPredictions(raw_pred.asBytes()); } - pm.send("driverStateV2", msg); + pm.send("driverState", msg); } void dmonitoring_free(DMonitoringModelState* s) { diff --git a/selfdrive/modeld/models/dmonitoring.h b/selfdrive/modeld/models/dmonitoring.h index 874722cd93..a1be91e3bb 100644 --- a/selfdrive/modeld/models/dmonitoring.h +++ b/selfdrive/modeld/models/dmonitoring.h @@ -9,43 +9,44 @@ #define CALIB_LEN 3 -#define OUTPUT_SIZE 84 +#define OUTPUT_SIZE 45 #define REG_SCALE 0.25f -typedef struct DriverStateResult { +typedef struct DMonitoringResult { float face_orientation[3]; - float face_orientation_std[3]; + float face_orientation_meta[3]; float face_position[2]; - float face_position_std[2]; + float face_position_meta[2]; float face_prob; float left_eye_prob; float right_eye_prob; float left_blink_prob; float right_blink_prob; - float sunglasses_prob; + float sg_prob; + float poor_vision; + float partial_face; + float distracted_pose; + float distracted_eyes; float occluded_prob; float ready_prob[4]; float not_ready_prob[2]; -} DriverStateResult; - -typedef struct DMonitoringModelResult { - DriverStateResult driver_state_lhd; - DriverStateResult driver_state_rhd; - float poor_vision_prob; - float wheel_on_right_prob; float dsp_execution_time; -} DMonitoringModelResult; +} DMonitoringResult; typedef struct DMonitoringModelState { RunModel *m; bool is_rhd; float output[OUTPUT_SIZE]; - std::vector net_input_buf; + std::vector resized_buf; + std::vector cropped_buf; + std::vector premirror_cropped_buf; + std::vector net_input_buf; float calib[CALIB_LEN]; + float tensor[UINT8_MAX + 1]; } DMonitoringModelState; void dmonitoring_init(DMonitoringModelState* s); -DMonitoringModelResult 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 DMonitoringModelResult &model_res, float execution_time, kj::ArrayPtr raw_pred); +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/dmonitoring_model.current b/selfdrive/modeld/models/dmonitoring_model.current index ba2865429e..74bcfe17a4 100644 --- a/selfdrive/modeld/models/dmonitoring_model.current +++ b/selfdrive/modeld/models/dmonitoring_model.current @@ -1,2 +1,2 @@ -5b02cff5-2b29-431d-b186-372e9c6fd0c7 -bf33cc569076984626ac7e027f927aa593261fa7 \ No newline at end of file +a8236e30-5bee-4689-8ea0-fc102e2770e5 +d508c79bae1c1c451f3af3e2bc231ce33678cb43 \ No newline at end of file diff --git a/selfdrive/modeld/models/dmonitoring_model.onnx b/selfdrive/modeld/models/dmonitoring_model.onnx index 1fca1317d5..51b0d1ed76 100644 --- a/selfdrive/modeld/models/dmonitoring_model.onnx +++ b/selfdrive/modeld/models/dmonitoring_model.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:aca1dd411b5f488bea605dc360656e631fc4301968a589ea072e2220c8092600 -size 9157561 +oid sha256:00731ebd06fcff7e5837607b91bc56cad3bed5d7ee89052c911c981e8f665308 +size 3679940 diff --git a/selfdrive/modeld/models/dmonitoring_model_q.dlc b/selfdrive/modeld/models/dmonitoring_model_q.dlc index c1cba15176..2e54f7ee4b 100644 --- a/selfdrive/modeld/models/dmonitoring_model_q.dlc +++ b/selfdrive/modeld/models/dmonitoring_model_q.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:d146b1d1fd9d40d57d058e51d285f83676866e26d9e5aff9fa27623ce343b58a -size 2636941 +oid sha256:667df5e925570a0f6a33dfb890e186a1f13f101885b46db47ec45305737dffb6 +size 1145921 diff --git a/selfdrive/modeld/runners/onnx_runner.py b/selfdrive/modeld/runners/onnx_runner.py index ac7cc68814..e282a66b66 100755 --- a/selfdrive/modeld/runners/onnx_runner.py +++ b/selfdrive/modeld/runners/onnx_runner.py @@ -9,24 +9,20 @@ os.environ["OMP_WAIT_POLICY"] = "PASSIVE" import onnxruntime as ort # pylint: disable=import-error -def read(sz, tf8=False): +def read(sz): dd = [] gt = 0 - szof = 1 if tf8 else 4 - while gt < sz * szof: - st = os.read(0, sz * szof - gt) + while gt < sz * 4: + st = os.read(0, sz * 4 - gt) assert(len(st) > 0) dd.append(st) gt += len(st) - r = np.frombuffer(b''.join(dd), dtype=np.uint8 if tf8 else np.float32).astype(np.float32) - if tf8: - r = r / 255. - return r + return np.frombuffer(b''.join(dd), dtype=np.float32) def write(d): os.write(1, d.tobytes()) -def run_loop(m, tf8_input=False): +def run_loop(m): ishapes = [[1]+ii.shape[1:] for ii in m.get_inputs()] keys = [x.name for x in m.get_inputs()] @@ -37,10 +33,10 @@ def run_loop(m, tf8_input=False): print("ready to run onnx model", keys, ishapes, file=sys.stderr) while 1: inputs = [] - for k, shp in zip(keys, ishapes): + for shp in ishapes: ts = np.product(shp) #print("reshaping %s with offset %d" % (str(shp), offset), file=sys.stderr) - inputs.append(read(ts, (k=='input_img' and tf8_input)).reshape(shp)) + inputs.append(read(ts).reshape(shp)) ret = m.run(None, dict(zip(keys, inputs))) #print(ret, file=sys.stderr) for r in ret: @@ -48,7 +44,6 @@ def run_loop(m, tf8_input=False): if __name__ == "__main__": - print(sys.argv, file=sys.stderr) print("Onnx available providers: ", ort.get_available_providers(), file=sys.stderr) options = ort.SessionOptions() options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_DISABLE_ALL @@ -68,6 +63,6 @@ if __name__ == "__main__": print("Onnx selected provider: ", [provider], file=sys.stderr) ort_session = ort.InferenceSession(sys.argv[1], options, providers=[provider]) print("Onnx using ", ort_session.get_providers(), file=sys.stderr) - run_loop(ort_session, tf8_input=("--use_tf8" in sys.argv)) + run_loop(ort_session) except KeyboardInterrupt: pass diff --git a/selfdrive/modeld/runners/onnxmodel.cc b/selfdrive/modeld/runners/onnxmodel.cc index 1f9f551abc..9b4d6fd015 100644 --- a/selfdrive/modeld/runners/onnxmodel.cc +++ b/selfdrive/modeld/runners/onnxmodel.cc @@ -14,13 +14,12 @@ #include "common/swaglog.h" #include "common/util.h" -ONNXModel::ONNXModel(const char *path, float *_output, size_t _output_size, int runtime, bool _use_extra, bool _use_tf8) { +ONNXModel::ONNXModel(const char *path, float *_output, size_t _output_size, int runtime, bool _use_extra) { LOGD("loading model %s", path); output = _output; output_size = _output_size; use_extra = _use_extra; - use_tf8 = _use_tf8; int err = pipe(pipein); assert(err == 0); @@ -29,12 +28,11 @@ ONNXModel::ONNXModel(const char *path, float *_output, size_t _output_size, int std::string exe_dir = util::dir_name(util::readlink("/proc/self/exe")); std::string onnx_runner = exe_dir + "/runners/onnx_runner.py"; - std::string tf8_arg = use_tf8 ? "--use_tf8" : ""; proc_pid = fork(); if (proc_pid == 0) { LOGD("spawning onnx process %s", onnx_runner.c_str()); - char *argv[] = {(char*)onnx_runner.c_str(), (char*)path, (char*)tf8_arg.c_str(), nullptr}; + char *argv[] = {(char*)onnx_runner.c_str(), (char*)path, nullptr}; dup2(pipein[0], 0); dup2(pipeout[1], 1); close(pipein[0]); diff --git a/selfdrive/modeld/runners/onnxmodel.h b/selfdrive/modeld/runners/onnxmodel.h index 4ac599e2af..567d81d29e 100644 --- a/selfdrive/modeld/runners/onnxmodel.h +++ b/selfdrive/modeld/runners/onnxmodel.h @@ -6,7 +6,7 @@ class ONNXModel : public RunModel { public: - ONNXModel(const char *path, float *output, size_t output_size, int runtime, bool use_extra = false, bool _use_tf8 = false); + ONNXModel(const char *path, float *output, size_t output_size, int runtime, bool use_extra = false); ~ONNXModel(); void addRecurrent(float *state, int state_size); void addDesire(float *state, int state_size); @@ -31,7 +31,6 @@ private: int calib_size; float *image_input_buf = NULL; int image_buf_size; - bool use_tf8; float *extra_input_buf = NULL; int extra_buf_size; bool use_extra; diff --git a/selfdrive/modeld/runners/snpemodel.cc b/selfdrive/modeld/runners/snpemodel.cc index 4d6917e894..1861494d59 100644 --- a/selfdrive/modeld/runners/snpemodel.cc +++ b/selfdrive/modeld/runners/snpemodel.cc @@ -14,11 +14,10 @@ void PrintErrorStringAndExit() { std::exit(EXIT_FAILURE); } -SNPEModel::SNPEModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra, bool luse_tf8) { +SNPEModel::SNPEModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra) { output = loutput; output_size = loutput_size; use_extra = luse_extra; - use_tf8 = luse_tf8; #ifdef QCOM2 if (runtime==USE_GPU_RUNTIME) { Runtime = zdl::DlSystem::Runtime_t::GPU; @@ -71,16 +70,14 @@ SNPEModel::SNPEModel(const char *path, float *loutput, size_t loutput_size, int printf("model: %s -> %s\n", input_tensor_name, output_tensor_name); zdl::DlSystem::UserBufferEncodingFloat userBufferEncodingFloat; - zdl::DlSystem::UserBufferEncodingTf8 userBufferEncodingTf8(0, 1./255); // network takes 0-1 zdl::DlSystem::IUserBufferFactory& ubFactory = zdl::SNPE::SNPEFactory::getUserBufferFactory(); - size_t size_of_input = use_tf8 ? sizeof(uint8_t) : sizeof(float); // create input buffer { const auto &inputDims_opt = snpe->getInputDimensions(input_tensor_name); const zdl::DlSystem::TensorShape& bufferShape = *inputDims_opt; std::vector strides(bufferShape.rank()); - strides[strides.size() - 1] = size_of_input; + strides[strides.size() - 1] = sizeof(float); size_t product = 1; for (size_t i = 0; i < bufferShape.rank(); i++) product *= bufferShape[i]; size_t stride = strides[strides.size() - 1]; @@ -89,10 +86,7 @@ SNPEModel::SNPEModel(const char *path, float *loutput, size_t loutput_size, int strides[i-1] = stride; } printf("input product is %lu\n", product); - inputBuffer = ubFactory.createUserBuffer(NULL, - product*size_of_input, - strides, - use_tf8 ? (zdl::DlSystem::UserBufferEncoding*)&userBufferEncodingTf8 : (zdl::DlSystem::UserBufferEncoding*)&userBufferEncodingFloat); + inputBuffer = ubFactory.createUserBuffer(NULL, product*sizeof(float), strides, &userBufferEncodingFloat); inputMap.add(input_tensor_name, inputBuffer.get()); } diff --git a/selfdrive/modeld/runners/snpemodel.h b/selfdrive/modeld/runners/snpemodel.h index ed9d58d1e1..ba51fdced0 100644 --- a/selfdrive/modeld/runners/snpemodel.h +++ b/selfdrive/modeld/runners/snpemodel.h @@ -23,7 +23,7 @@ class SNPEModel : public RunModel { public: - SNPEModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra = false, bool use_tf8 = false); + SNPEModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra = false); void addRecurrent(float *state, int state_size); void addTrafficConvention(float *state, int state_size); void addCalib(float *state, int state_size); @@ -52,7 +52,6 @@ private: std::unique_ptr inputBuffer; float *input; size_t input_size; - bool use_tf8; // snpe output stuff zdl::DlSystem::UserBufferMap outputMap; diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index c31e9da43b..9a3196030b 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -18,7 +18,7 @@ def dmonitoringd_thread(sm=None, pm=None): pm = messaging.PubMaster(['driverMonitoringState']) if sm is None: - sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll=['driverStateV2']) + sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll=['driverState']) driver_status = DriverStatus(rhd=Params().get_bool("IsRHD")) @@ -34,7 +34,7 @@ def dmonitoringd_thread(sm=None, pm=None): while True: sm.update() - if not sm.updated['driverStateV2']: + if not sm.updated['driverState']: continue # Get interaction @@ -51,7 +51,7 @@ def dmonitoringd_thread(sm=None, pm=None): # Get data from dmonitoringmodeld events = Events() - driver_status.update_states(sm['driverStateV2'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled) + driver_status.update_states(sm['driverState'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled) # Block engaging after max number of distrations if driver_status.terminal_alert_cnt >= driver_status.settings._MAX_TERMINAL_ALERTS or \ @@ -79,7 +79,6 @@ def dmonitoringd_thread(sm=None, pm=None): "isLowStd": driver_status.pose.low_std, "hiStdCount": driver_status.hi_stds, "isActiveMode": driver_status.active_monitoring_mode, - "isRHD": driver_status.wheel_on_right, } pm.send('driverMonitoringState', dat) diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py index e7ed175846..662e0d76ce 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/selfdrive/monitoring/driver_monitor.py @@ -5,7 +5,6 @@ from common.numpy_fast import interp from common.realtime import DT_DMON from common.filter_simple import FirstOrderFilter from common.stat_live import RunningStatFilter -from common.transformations.camera import tici_d_frame_size EventName = car.CarEvent.EventName @@ -27,29 +26,32 @@ class DRIVER_MONITOR_SETTINGS(): self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6. self._FACE_THRESHOLD = 0.5 + self._PARTIAL_FACE_THRESHOLD = 0.8 self._EYE_THRESHOLD = 0.65 self._SG_THRESHOLD = 0.925 - self._BLINK_THRESHOLD = 0.861 + self._BLINK_THRESHOLD = 0.8 + self._BLINK_THRESHOLD_SLACK = 0.9 + self._BLINK_THRESHOLD_STRICT = self._BLINK_THRESHOLD self._EE_THRESH11 = 0.75 self._EE_THRESH12 = 3.25 self._EE_THRESH21 = 0.01 self._EE_THRESH22 = 0.35 - self._POSE_PITCH_THRESHOLD = 0.3133 - self._POSE_PITCH_THRESHOLD_SLACK = 0.3237 + self._POSE_PITCH_THRESHOLD = 0.3237 + self._POSE_PITCH_THRESHOLD_SLACK = 0.3657 self._POSE_PITCH_THRESHOLD_STRICT = self._POSE_PITCH_THRESHOLD - self._POSE_YAW_THRESHOLD = 0.4020 - self._POSE_YAW_THRESHOLD_SLACK = 0.5042 + self._POSE_YAW_THRESHOLD = 0.3109 + self._POSE_YAW_THRESHOLD_SLACK = 0.4294 self._POSE_YAW_THRESHOLD_STRICT = self._POSE_YAW_THRESHOLD - self._PITCH_NATURAL_OFFSET = 0.029 # initial value before offset is learned - self._YAW_NATURAL_OFFSET = 0.097 # initial value before offset is learned + self._PITCH_NATURAL_OFFSET = 0.057 # initial value before offset is learned + self._YAW_NATURAL_OFFSET = 0.11 # initial value before offset is learned self._PITCH_MAX_OFFSET = 0.124 self._PITCH_MIN_OFFSET = -0.0881 self._YAW_MAX_OFFSET = 0.289 self._YAW_MIN_OFFSET = -0.0246 - self._POSESTD_THRESHOLD = 0.3 + self._POSESTD_THRESHOLD = 0.315 self._HI_STD_FALLBACK_TIME = int(10 / self._DT_DMON) # fall back to wheel touch if model is uncertain for 10s self._DISTRACTED_FILTER_TS = 0.25 # 0.6Hz @@ -57,9 +59,6 @@ class DRIVER_MONITOR_SETTINGS(): self._POSE_OFFSET_MIN_COUNT = int(60 / self._DT_DMON) # valid data counts before calibration completes, 1min cumulative self._POSE_OFFSET_MAX_COUNT = int(360 / self._DT_DMON) # stop deweighting new data after 6 min, aka "short term memory" - self._WHEELPOS_THRESHOLD = 0.5 - self._WHEELPOS_FILTER_MIN_COUNT = int(5 / self._DT_DMON) - self._RECOVERY_FACTOR_MAX = 5. # relative to minus step change self._RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change @@ -67,9 +66,9 @@ class DRIVER_MONITOR_SETTINGS(): self._MAX_TERMINAL_DURATION = int(30 / self._DT_DMON) # not allowed to engage after 30s of terminal alerts -# model output refers to center of undistorted+leveled image -EFL = 598.0 # focal length in K -W, H = tici_d_frame_size # corrected image has same size as raw +# model output refers to center of cropped image, so need to apply the x displacement offset +RESIZED_FOCAL = 320.0 +H, W, FULL_W = 320, 160, 426 class DistractedType: NOT_DISTRACTED = 0 @@ -77,22 +76,22 @@ class DistractedType: DISTRACTED_BLINK = 2 DISTRACTED_E2E = 4 -def face_orientation_from_net(angles_desc, pos_desc, rpy_calib): +def face_orientation_from_net(angles_desc, pos_desc, rpy_calib, is_rhd): # the output of these angles are in device frame # so from driver's perspective, pitch is up and yaw is right pitch_net, yaw_net, roll_net = angles_desc - face_pixel_position = ((pos_desc[0]+0.5)*W, (pos_desc[1]+0.5)*H) - yaw_focal_angle = atan2(face_pixel_position[0] - W//2, EFL) - pitch_focal_angle = atan2(face_pixel_position[1] - H//2, EFL) + face_pixel_position = ((pos_desc[0] + .5)*W - W + FULL_W, (pos_desc[1]+.5)*H) + yaw_focal_angle = atan2(face_pixel_position[0] - FULL_W//2, RESIZED_FOCAL) + pitch_focal_angle = atan2(face_pixel_position[1] - H//2, RESIZED_FOCAL) pitch = pitch_net + pitch_focal_angle yaw = -yaw_net + yaw_focal_angle # no calib for roll pitch -= rpy_calib[1] - yaw -= rpy_calib[2] + yaw -= rpy_calib[2] * (1 - 2 * int(is_rhd)) # lhd -> -=, rhd -> += return roll_net, pitch, yaw class DriverPose(): @@ -113,6 +112,7 @@ class DriverBlink(): def __init__(self): self.left_blink = 0. self.right_blink = 0. + self.cfactor = 1. class DriverStatus(): def __init__(self, rhd=False, settings=DRIVER_MONITOR_SETTINGS()): @@ -120,7 +120,7 @@ class DriverStatus(): self.settings = settings # init driver status - # self.wheelpos_learner = RunningStatFilter() + self.is_rhd_region = rhd self.pose = DriverPose(self.settings._POSE_OFFSET_MAX_COUNT) self.pose_calibrated = False self.blink = DriverBlink() @@ -137,8 +137,8 @@ class DriverStatus(): self.distracted_types = [] self.driver_distracted = False self.driver_distraction_filter = FirstOrderFilter(0., self.settings._DISTRACTED_FILTER_TS, self.settings._DT_DMON) - self.wheel_on_right = rhd self.face_detected = False + self.face_partial = False self.terminal_alert_cnt = 0 self.terminal_time = 0 self.step_change = 0. @@ -197,7 +197,7 @@ class DriverStatus(): yaw_error > self.settings._POSE_YAW_THRESHOLD*self.pose.cfactor_yaw: distracted_types.append(DistractedType.DISTRACTED_POSE) - if (self.blink.left_blink + self.blink.right_blink)*0.5 > self.settings._BLINK_THRESHOLD: + if (self.blink.left_blink + self.blink.right_blink)*0.5 > self.settings._BLINK_THRESHOLD*self.blink.cfactor: distracted_types.append(DistractedType.DISTRACTED_BLINK) if self.ee1_calibrated: @@ -214,7 +214,13 @@ class DriverStatus(): return distracted_types def set_policy(self, model_data, car_speed): + ep = min(model_data.meta.engagedProb, 0.8) / 0.8 # engaged prob bp = model_data.meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s + # TODO: retune adaptive blink + self.blink.cfactor = interp(ep, [0, 0.5, 1], + [self.settings._BLINK_THRESHOLD_STRICT, + self.settings._BLINK_THRESHOLD, + self.settings._BLINK_THRESHOLD_SLACK]) / self.settings._BLINK_THRESHOLD k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2) bp_normal = max(min(bp / k1, 0.5),0) self.pose.cfactor_pitch = interp(bp_normal, [0, 0.5], @@ -225,36 +231,28 @@ class DriverStatus(): self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD def update_states(self, driver_state, cal_rpy, car_speed, op_engaged): - # rhd_pred = driver_state.wheelOnRightProb - # if car_speed > 0.01: - # self.wheelpos_learner.push_and_update(rhd_pred) - # if self.wheelpos_learner.filtered_stat.n > self.settings._WHEELPOS_FILTER_MIN_COUNT: - # self.wheel_on_right = self.wheelpos_learner.filtered_stat.M > self.settings._WHEELPOS_THRESHOLD - # else: - # self.wheel_on_right = rhd_pred > self.settings._WHEELPOS_THRESHOLD - driver_data = driver_state.rightDriverData if self.wheel_on_right else driver_state.leftDriverData - if not all(len(x) > 0 for x in (driver_data.faceOrientation, driver_data.facePosition, - driver_data.faceOrientationStd, driver_data.facePositionStd, - driver_data.readyProb, driver_data.notReadyProb)): + if not all(len(x) > 0 for x in (driver_state.faceOrientation, driver_state.facePosition, + driver_state.faceOrientationStd, driver_state.facePositionStd, + driver_state.readyProb, driver_state.notReadyProb)): return - self.face_detected = driver_data.faceProb > self.settings._FACE_THRESHOLD - self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_data.faceOrientation, driver_data.facePosition, cal_rpy) - if self.wheel_on_right: - self.pose.yaw *= -1 - self.pose.pitch_std = driver_data.faceOrientationStd[0] - self.pose.yaw_std = driver_data.faceOrientationStd[1] + self.face_partial = driver_state.partialFace > self.settings._PARTIAL_FACE_THRESHOLD + self.face_detected = driver_state.faceProb > self.settings._FACE_THRESHOLD or self.face_partial + self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_state.faceOrientation, driver_state.facePosition, cal_rpy, self.is_rhd_region) + self.pose.pitch_std = driver_state.faceOrientationStd[0] + self.pose.yaw_std = driver_state.faceOrientationStd[1] + # self.pose.roll_std = driver_state.faceOrientationStd[2] model_std_max = max(self.pose.pitch_std, self.pose.yaw_std) - self.pose.low_std = model_std_max < self.settings._POSESTD_THRESHOLD - self.blink.left_blink = driver_data.leftBlinkProb * (driver_data.leftEyeProb > self.settings._EYE_THRESHOLD) * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD) - self.blink.right_blink = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD) - self.eev1 = driver_data.notReadyProb[1] - self.eev2 = driver_data.readyProb[0] + self.pose.low_std = model_std_max < self.settings._POSESTD_THRESHOLD and not self.face_partial + self.blink.left_blink = driver_state.leftBlinkProb * (driver_state.leftEyeProb > self.settings._EYE_THRESHOLD) * (driver_state.sunglassesProb < self.settings._SG_THRESHOLD) + self.blink.right_blink = driver_state.rightBlinkProb * (driver_state.rightEyeProb > self.settings._EYE_THRESHOLD) * (driver_state.sunglassesProb < self.settings._SG_THRESHOLD) + self.eev1 = driver_state.notReadyProb[1] + self.eev2 = driver_state.readyProb[0] self.distracted_types = self._get_distracted_types() self.driver_distracted = (DistractedType.DISTRACTED_POSE in self.distracted_types or DistractedType.DISTRACTED_BLINK in self.distracted_types) and \ - driver_data.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std + driver_state.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std self.driver_distraction_filter.update(self.driver_distracted) # update offseter diff --git a/selfdrive/monitoring/test_monitoring.py b/selfdrive/monitoring/test_monitoring.py index 43b5e7747e..a84ed242ba 100755 --- a/selfdrive/monitoring/test_monitoring.py +++ b/selfdrive/monitoring/test_monitoring.py @@ -17,19 +17,19 @@ INVISIBLE_SECONDS_TO_ORANGE = dm_settings._AWARENESS_TIME - dm_settings._AWARENE INVISIBLE_SECONDS_TO_RED = dm_settings._AWARENESS_TIME + 1 def make_msg(face_detected, distracted=False, model_uncertain=False): - ds = log.DriverStateV2.new_message() - ds.leftDriverData.faceOrientation = [0., 0., 0.] - ds.leftDriverData.facePosition = [0., 0.] - ds.leftDriverData.faceProb = 1. * face_detected - ds.leftDriverData.leftEyeProb = 1. - ds.leftDriverData.rightEyeProb = 1. - ds.leftDriverData.leftBlinkProb = 1. * distracted - ds.leftDriverData.rightBlinkProb = 1. * distracted - ds.leftDriverData.faceOrientationStd = [1.*model_uncertain, 1.*model_uncertain, 1.*model_uncertain] - ds.leftDriverData.facePositionStd = [1.*model_uncertain, 1.*model_uncertain] + ds = log.DriverState.new_message() + ds.faceOrientation = [0., 0., 0.] + ds.facePosition = [0., 0.] + ds.faceProb = 1. * face_detected + ds.leftEyeProb = 1. + ds.rightEyeProb = 1. + ds.leftBlinkProb = 1. * distracted + ds.rightBlinkProb = 1. * distracted + ds.faceOrientationStd = [1.*model_uncertain, 1.*model_uncertain, 1.*model_uncertain] + ds.facePositionStd = [1.*model_uncertain, 1.*model_uncertain] # TODO: test both separately when e2e is used - ds.leftDriverData.readyProb = [0., 0., 0., 0.] - ds.leftDriverData.notReadyProb = [0., 0.] + ds.readyProb = [0., 0., 0., 0.] + ds.notReadyProb = [0., 0.] return ds diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py index 032b504879..b83629c76a 100755 --- a/selfdrive/test/process_replay/model_replay.py +++ b/selfdrive/test/process_replay/model_replay.py @@ -52,7 +52,7 @@ def model_replay(lr, frs): vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, *(tici_f_frame_size)) vipc_server.start_listener() - sm = messaging.SubMaster(['modelV2', 'driverStateV2']) + sm = messaging.SubMaster(['modelV2', 'driverState']) pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'driverCameraState', 'liveCalibration', 'lateralPlan']) try: @@ -112,7 +112,7 @@ def model_replay(lr, frs): if min(frame_idxs['roadCameraState'], frame_idxs['wideRoadCameraState']) > recv_cnt['modelV2']: recv = "modelV2" elif msg.which() == 'driverCameraState': - recv = "driverStateV2" + recv = "driverState" # wait for a response with Timeout(15, f"timed out waiting for {recv}"): @@ -170,8 +170,8 @@ if __name__ == "__main__": 'logMonoTime', 'modelV2.frameDropPerc', 'modelV2.modelExecutionTime', - 'driverStateV2.modelExecutionTime', - 'driverStateV2.dspExecutionTime' + 'driverState.modelExecutionTime', + 'driverState.dspExecutionTime' ] # TODO this tolerence is absurdly large tolerance = 5e-1 if PC else None diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index 626d54a8f0..90520f2619 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -1d0979ca59dbc89bc5890656e9501e83f0556d50 +f74ab97371be93fdc28333e5ea12bbb78c3a32d0 diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 89885ef397..1eda9bc7f5 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -293,7 +293,7 @@ CONFIGS = [ ProcessConfig( proc_name="dmonitoringd", pub_sub={ - "driverStateV2": ["driverMonitoringState"], + "driverState": ["driverMonitoringState"], "liveCalibration": [], "carState": [], "modelV2": [], "controlsState": [], }, ignore=["logMonoTime", "valid"], diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 3beeaff3b2..6cf53a046e 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -33,7 +33,7 @@ PROCS = { "selfdrive.controls.radard": 4.5, "./_modeld": 4.48, "./boardd": 3.63, - "./_dmonitoringmodeld": 5.0, + "./_dmonitoringmodeld": 10.0, "selfdrive.thermald.thermald": 3.87, "selfdrive.locationd.calibrationd": 2.0, "./_soundd": 1.0, @@ -60,7 +60,7 @@ TIMINGS = { "roadCameraState": [2.5, 0.35], "driverCameraState": [2.5, 0.35], "modelV2": [2.5, 0.35], - "driverStateV2": [2.5, 0.40], + "driverState": [2.5, 0.40], "liveLocationKalman": [2.5, 0.35], "wideRoadCameraState": [1.5, 0.35], } @@ -221,7 +221,7 @@ class TestOnroad(unittest.TestCase): # TODO: this went up when plannerd cpu usage increased, why? cfgs = [ ("modelV2", 0.050, 0.036), - ("driverStateV2", 0.050, 0.026), + ("driverState", 0.050, 0.026), ] for (s, instant_max, avg_max) in cfgs: ts = [getattr(getattr(m, s), "modelExecutionTime") for m in self.lr if m.which() == s] diff --git a/selfdrive/ui/qt/offroad/driverview.cc b/selfdrive/ui/qt/offroad/driverview.cc index 868a97f604..06578b455a 100644 --- a/selfdrive/ui/qt/offroad/driverview.cc +++ b/selfdrive/ui/qt/offroad/driverview.cc @@ -25,7 +25,7 @@ void DriverViewWindow::mouseReleaseEvent(QMouseEvent* e) { emit done(); } -DriverViewScene::DriverViewScene(QWidget* parent) : sm({"driverStateV2"}), QWidget(parent) { +DriverViewScene::DriverViewScene(QWidget* parent) : sm({"driverState"}), QWidget(parent) { face_img = loadPixmap("../assets/img_driver_face.png", {FACE_IMG_SIZE, FACE_IMG_SIZE}); } @@ -57,36 +57,43 @@ void DriverViewScene::paintEvent(QPaintEvent* event) { return; } - cereal::DriverStateV2::Reader driver_state = sm["driverStateV2"].getDriverStateV2(); - cereal::DriverStateV2::DriverData::Reader driver_data; + const int width = 4 * height() / 3; + const QRect rect2 = {rect().center().x() - width / 2, rect().top(), width, rect().height()}; + const QRect valid_rect = {is_rhd ? rect2.right() - rect2.height() / 2 : rect2.left(), rect2.top(), rect2.height() / 2, rect2.height()}; - // is_rhd = driver_state.getWheelOnRightProb() > 0.5; - driver_data = is_rhd ? driver_state.getRightDriverData() : driver_state.getLeftDriverData(); + // blackout + const QColor bg(0, 0, 0, 140); + 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); + 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 - bool face_detected = driver_data.getFaceProb() > 0.5; + // face bounding box + cereal::DriverState::Reader driver_state = sm["driverState"].getDriverState(); + bool face_detected = driver_state.getFaceProb() > 0.5; if (face_detected) { - auto fxy_list = driver_data.getFacePosition(); - auto std_list = driver_data.getFaceOrientationStd(); + auto fxy_list = driver_state.getFacePosition(); + auto std_list = driver_state.getFaceOrientationStd(); float face_x = fxy_list[0]; float face_y = fxy_list[1]; float face_std = std::max(std_list[0], std_list[1]); float alpha = 0.7; - if (face_std > 0.15) { - alpha = std::max(0.7 - (face_std-0.15)*3.5, 0.0); + if (face_std > 0.08) { + alpha = std::max(0.7 - (face_std-0.08)*7, 0.0); } - const int box_size = 220; - // use approx instead of distort_points - int fbox_x = 1080.0 - 1714.0 * face_x; - int fbox_y = -135.0 + (504.0 + std::abs(face_x)*112.0) + (1205.0 - std::abs(face_x)*724.0) * face_y; + const int box_size = 0.6 * rect2.height() / 2; + const float rhd_offset = 0.05; // lhd is shifted, so rhd is not mirrored + int fbox_x = valid_rect.center().x() + (is_rhd ? (face_x + rhd_offset) : -face_x) * valid_rect.width(); + int fbox_y = valid_rect.center().y() + face_y * valid_rect.height(); p.setPen(QPen(QColor(255, 255, 255, alpha * 255), 10)); p.drawRoundedRect(fbox_x - box_size / 2, fbox_y - box_size / 2, box_size, box_size, 35.0, 35.0); } // icon - const int img_offset = 60; - const int img_x = rect().left() + img_offset; - const int img_y = rect().bottom() - FACE_IMG_SIZE - img_offset; - p.setOpacity(face_detected ? 1.0 : 0.2); + const int img_offset = 30; + const int img_x = is_rhd ? rect2.right() - FACE_IMG_SIZE - img_offset : rect2.left() + img_offset; + const int img_y = rect2.bottom() - FACE_IMG_SIZE - img_offset; + p.setOpacity(face_detected ? 1.0 : 0.3); p.drawPixmap(img_x, img_y, face_img); } diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index 0a3b9762e5..f16e8e4e0d 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -54,15 +54,16 @@ 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 = 2.0; - const float yscale = stream_height * driver_view_ratio / stream_width; + const float driver_view_ratio = 1.333; + 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, 0.0, - 0.0, yscale, 0.0, 0.0, + 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; } From 6618d2bebee9a1fc5a0ee5d5a9b2764403865281 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 10 Jun 2022 16:18:31 -0700 Subject: [PATCH 34/93] Add missing fw versions for 2019 Sonata (#24814) --- selfdrive/car/hyundai/values.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 7d3ab93cde..4ca81fdb40 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -501,6 +501,7 @@ FW_VERSIONS = { ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00LFF LKAS AT USA LHD 1.00 1.01 95740-C1000 E51', + b'\xf1\x00LFF LKAS AT USA LHD 1.01 1.02 95740-C1000 E52', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x006T6H0_C2\x00\x006T6B4051\x00\x00TLF0G24NL1\xb0\x9f\xee\xf5', @@ -509,6 +510,7 @@ FW_VERSIONS = { b'\xf1\x87\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xf1\x816T6B4051\x00\x00\xf1\x006T6H0_C2\x00\x006T6B4051\x00\x00TLF0G24SL2n\x8d\xbe\xd8', b'\xf1\x87LAHSGN012918KF10\x98\x88x\x87\x88\x88x\x87\x88\x88\x98\x88\x87w\x88w\x88\x88\x98\x886o\xf6\xff\x98w\x7f\xff3\x00\xf1\x816W3B1051\x00\x00\xf1\x006W351_C2\x00\x006W3B1051\x00\x00TLF0T20NL2\x00\x00\x00\x00', b'\xf1\x87LAHSGN012918KF10\x98\x88x\x87\x88\x88x\x87\x88\x88\x98\x88\x87w\x88w\x88\x88\x98\x886o\xf6\xff\x98w\x7f\xff3\x00\xf1\x816W3B1051\x00\x00\xf1\x006W351_C2\x00\x006W3B1051\x00\x00TLF0T20NL2H\r\xbdm', + b'\xf1\x87LAJSG49645724HF0\x87x\x87\x88\x87www\x88\x99\xa8\x89\x88\x99\xa8\x89\x88\x99\xa8\x89S_\xfb\xff\x87f\x7f\xff^2\xf1\x816W3B1051\x00\x00\xf1\x006W351_C2\x00\x006W3B1051\x00\x00TLF0T20NL2H\r\xbdm', ], }, CAR.TUCSON: { From a8ccd8f838aff958a60546de04bf42220567b288 Mon Sep 17 00:00:00 2001 From: ZwX1616 Date: Fri, 10 Jun 2022 16:26:06 -0700 Subject: [PATCH 35/93] put cereal on master --- cereal | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cereal b/cereal index e9d3597d23..ff49c8e126 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit e9d3597d23311a3e33a2def74ceec839e5ff4bf5 +Subproject commit ff49c8e126ea04889af13d690ceaf520ce7084d2 From 3066ad81a8b2f89a22ddfc86a879395c573ce0f5 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Fri, 10 Jun 2022 17:57:23 -0700 Subject: [PATCH 36/93] Car interface: set max lateral torque from table (#24789) * json * better naem * Read from table * formatting and default to nan * Generate docs * Read from table * this should be the same * Prius v is full * test we always set the tunes correctly add to release files * Set for all cars Set for all cars * Revert tuning changes Revert tuning changes * remove that * fixes * update ref commit for new maxLateralAccels Co-authored-by: Shane Smiskol --- docs/CARS.md | 68 +++++++++++----------- release/files_common | 1 + selfdrive/car/chrysler/interface.py | 8 --- selfdrive/car/docs_definitions.py | 2 +- selfdrive/car/gm/interface.py | 2 - selfdrive/car/honda/interface.py | 11 ---- selfdrive/car/hyundai/interface.py | 8 --- selfdrive/car/interfaces.py | 10 +++- selfdrive/car/subaru/interface.py | 2 - selfdrive/car/tests/test_car_interfaces.py | 5 +- selfdrive/car/tests/test_docs.py | 6 ++ selfdrive/car/torque_data.json | 1 + selfdrive/car/toyota/interface.py | 12 ---- selfdrive/car/volkswagen/interface.py | 6 -- selfdrive/test/process_replay/ref_commit | 2 +- 15 files changed, 57 insertions(+), 87 deletions(-) create mode 100644 selfdrive/car/torque_data.json diff --git a/docs/CARS.md b/docs/CARS.md index e9b9e947f3..479c519dab 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -35,7 +35,7 @@ How We Rate The Cars **All supported cars can move between the tiers as support changes.** -# Gold - 29 cars +# Gold - 28 cars |Make|Model|Supported Package|openpilot ACC|Stop and Go|Steer to 0|Steering Torque|Actively Maintained| |---|---|---|:---:|:---:|:---:|:---:|:---:| @@ -52,10 +52,9 @@ How We Rate The Cars |Lexus|ES 2019-21|All|||||| |Lexus|ES Hybrid 2019-22|All|||||| |Lexus|NX 2020|All|||||| -|Lexus|NX Hybrid 2020|All|||||| +|Lexus|RX 2020-22|All|||||| |Lexus|UX Hybrid 2019-21|All|||||| |Toyota|Avalon 2022|All|||||| -|Toyota|Avalon Hybrid 2022|All|||||| |Toyota|Camry 2021-22|All||[4](#footnotes)|||| |Toyota|Camry Hybrid 2021-22|All|||||| |Toyota|Corolla 2020-22|All|||||| @@ -69,14 +68,13 @@ How We Rate The Cars |Toyota|RAV4 2019-21|All|||||| |Toyota|RAV4 Hybrid 2019-21|All|||||| -# Silver - 78 cars +# Silver - 75 cars |Make|Model|Supported Package|openpilot ACC|Stop and Go|Steer to 0|Steering Torque|Actively Maintained| |---|---|---|:---:|:---:|:---:|:---:|:---:| |Audi|A3 2014-19|ACC + Lane Assist|||||| |Audi|A3 Sportback e-tron 2017-18|ACC + Lane Assist|||||| |Audi|Q2 2018|ACC + Lane Assist|||||| -|Audi|Q3 2020-21|ACC + Lane Assist|||||| |Audi|RS3 2018|ACC + Lane Assist|||||| |Audi|S3 2015-17|ACC + Lane Assist|||||| |Chevrolet|Volt 2017-18[1](#footnotes)|Adaptive Cruise|||||| @@ -93,7 +91,6 @@ How We Rate The Cars |Hyundai|Santa Fe 2021-22|All|||||| |Hyundai|Santa Fe Hybrid 2022|All|||||| |Hyundai|Santa Fe Plug-in Hybrid 2022|All|||||| -|Hyundai|Sonata 2018-19|SCC + LKAS|||||| |Hyundai|Tucson Diesel 2019|SCC + LKAS|||||| |Kia|Ceed 2019|SCC + LKAS|||||| |Kia|Forte 2018|SCC + LKAS|||||| @@ -106,15 +103,16 @@ How We Rate The Cars |Kia|Sorento 2018|SCC + LKAS|||||| |Kia|Sorento 2019|SCC + LKAS|||||| |Kia|Stinger 2018|SCC + LKAS|||||| -|Lexus|ES Hybrid 2017-18|LSS|[3](#footnotes)||||| |Lexus|NX 2018-19|All|[3](#footnotes)||||| |Lexus|NX Hybrid 2018-19|All|[3](#footnotes)||||| -|Lexus|RX 2020-22|All|||||| +|Lexus|NX Hybrid 2020|All|||||| |Lexus|RX Hybrid 2020-21|All|||||| |Mazda|CX-5 2022|All|||||| |SEAT|Ateca 2018|Driver Assistance|||||| |SEAT|Leon 2014-20|Driver Assistance|||||| +|Subaru|Crosstrek 2020-21|EyeSight|||||| |Subaru|Forester 2019-21|All|||||| +|Subaru|Impreza 2020-21|EyeSight|||||| |Škoda|Kamiq 2021[5](#footnotes)|Driver Assistance|||||| |Škoda|Karoq 2019|Driver Assistance|||||| |Škoda|Kodiaq 2018-19|Driver Assistance|||||| @@ -125,18 +123,17 @@ How We Rate The Cars |Toyota|Alphard 2019-20|All|||||| |Toyota|Alphard Hybrid 2021|All|||||| |Toyota|Avalon 2019-21|TSS-P|[3](#footnotes)||||| -|Toyota|Avalon Hybrid 2019-21|TSS-P|[3](#footnotes)||||| +|Toyota|Avalon Hybrid 2022|All|||||| |Toyota|Camry 2018-20|All||[4](#footnotes)|||| |Toyota|Camry Hybrid 2018-20|All||[4](#footnotes)|||| |Toyota|Highlander 2017-19|All|[3](#footnotes)||||| |Toyota|Highlander Hybrid 2017-19|All|[3](#footnotes)||||| |Toyota|Prius 2016-20|TSS-P|[3](#footnotes)||||| |Toyota|Prius Prime 2017-20|All|[3](#footnotes)||||| -|Toyota|RAV4 2022|All|||||| |Toyota|RAV4 Hybrid 2016-18|TSS-P|[3](#footnotes)||||| |Toyota|RAV4 Hybrid 2022|All|||||| |Toyota|Sienna 2018-20|All|[3](#footnotes)||||| -|Volkswagen|Arteon 2018, 2021[7](#footnotes)|Driver Assistance|||||| +|Volkswagen|Atlas 2018-19, 2022[7](#footnotes)|Driver Assistance|||||| |Volkswagen|e-Golf 2014, 2018-20|Driver Assistance|||||| |Volkswagen|Golf 2015-20|Driver Assistance|||||| |Volkswagen|Golf Alltrack 2017-18|Driver Assistance|||||| @@ -145,57 +142,59 @@ How We Rate The Cars |Volkswagen|Golf R 2016-19|Driver Assistance|||||| |Volkswagen|Golf SportsVan 2016|Driver Assistance|||||| |Volkswagen|Golf SportWagen 2015|Driver Assistance|||||| -|Volkswagen|Passat 2015-19[6](#footnotes)|Driver Assistance|||||| |Volkswagen|Polo 2020|Driver Assistance|||||| |Volkswagen|T-Cross 2021[7](#footnotes)|Driver Assistance|||||| |Volkswagen|T-Roc 2021[7](#footnotes)|Driver Assistance|||||| |Volkswagen|Taos 2022[7](#footnotes)|Driver Assistance|||||| |Volkswagen|Touran 2017|Driver Assistance|||||| -# Bronze - 66 cars +# Bronze - 70 cars |Make|Model|Supported Package|openpilot ACC|Stop and Go|Steer to 0|Steering Torque|Actively Maintained| |---|---|---|:---:|:---:|:---:|:---:|:---:| |Acura|ILX 2016-19|AcuraWatch Plus|||||| |Acura|RDX 2016-18|AcuraWatch Plus|||||| -|Acura|RDX 2019-21|All|||||| +|Acura|RDX 2019-21|All|||||| +|Audi|Q3 2020-21|ACC + Lane Assist|||||| |Cadillac|Escalade ESV 2016[1](#footnotes)|ACC + LKAS|||||| -|Chrysler|Pacifica 2017-18|Adaptive Cruise|||||| -|Chrysler|Pacifica 2020|Adaptive Cruise|||||| +|Chrysler|Pacifica 2017-18|Adaptive Cruise|||||| +|Chrysler|Pacifica 2020|Adaptive Cruise|||||| |Chrysler|Pacifica Hybrid 2017-18|Adaptive Cruise|||||| -|Chrysler|Pacifica Hybrid 2019-21|Adaptive Cruise|||||| +|Chrysler|Pacifica Hybrid 2019-21|Adaptive Cruise|||||| |Genesis|G90 2018|All|||||| |GMC|Acadia 2018[1](#footnotes)|Adaptive Cruise|||||| -|Honda|Accord 2018-21|All|||||| -|Honda|Accord Hybrid 2018-21|All|||||| +|Honda|Accord 2018-21|All|||||| +|Honda|Accord Hybrid 2018-21|All|||||| |Honda|Civic 2016-18|Honda Sensing|||||| -|Honda|Civic 2019-20|All|||[2](#footnotes)||| -|Honda|Civic Hatchback 2017-21|Honda Sensing|||||| +|Honda|Civic 2019-20|All|||[2](#footnotes)||| +|Honda|Civic Hatchback 2017-21|Honda Sensing|||||| |Honda|CR-V 2015-16|Touring|||||| -|Honda|CR-V 2017-21|Honda Sensing|||||| +|Honda|CR-V 2017-21|Honda Sensing|||||| |Honda|CR-V Hybrid 2017-19|Honda Sensing|||||| |Honda|e 2020|All|||||| -|Honda|Fit 2018-19|Honda Sensing|||||| +|Honda|Fit 2018-19|Honda Sensing|||||| |Honda|Freed 2020|Honda Sensing|||||| |Honda|HR-V 2019-20|Honda Sensing|||||| -|Honda|Insight 2019-21|All|||||| -|Honda|Inspire 2018|All|||||| +|Honda|Insight 2019-21|All|||||| +|Honda|Inspire 2018|All|||||| |Honda|Odyssey 2018-20|Honda Sensing|||||| -|Honda|Passport 2019-21|All|||||| -|Honda|Pilot 2016-21|Honda Sensing|||||| -|Honda|Ridgeline 2017-22|Honda Sensing|||||| +|Honda|Passport 2019-21|All|||||| +|Honda|Pilot 2016-21|Honda Sensing|||||| +|Honda|Ridgeline 2017-22|Honda Sensing|||||| |Hyundai|Elantra 2017-19|SCC + LKAS|||||| |Hyundai|Genesis 2015-16|SCC + LKAS|||||| |Hyundai|Ioniq Electric 2019|SCC + LKAS|||||| |Hyundai|Ioniq Hybrid 2017-19|SCC + LKAS|||||| |Hyundai|Ioniq Plug-in Hybrid 2019|SCC + LKAS|||||| +|Hyundai|Sonata 2018-19|SCC + LKAS|||||| |Hyundai|Tucson 2021|SCC + LKAS|||||| |Hyundai|Veloster 2019-20|SCC + LKAS|||||| |Jeep|Grand Cherokee 2016-18|Adaptive Cruise|||||| -|Jeep|Grand Cherokee 2019-20|Adaptive Cruise|||||| +|Jeep|Grand Cherokee 2019-20|Adaptive Cruise|||||| |Kia|Niro Plug-in Hybrid 2019|SCC + LKAS|||||| |Kia|Optima 2017|SCC + LKAS|||||| |Lexus|CT Hybrid 2017-18|LSS|[3](#footnotes)||||| +|Lexus|ES Hybrid 2017-18|LSS|[3](#footnotes)||||| |Lexus|IS 2017-19|All|||||| |Lexus|RC 2020|All|||||| |Lexus|RX 2016-18|All|[3](#footnotes)||||| @@ -207,20 +206,21 @@ How We Rate The Cars |Nissan|X-Trail 2017|ProPILOT|||||| |Subaru|Ascent 2019-20|All|||||| |Subaru|Crosstrek 2018-19|EyeSight|||||| -|Subaru|Crosstrek 2020-21|EyeSight|||||| |Subaru|Impreza 2017-19|EyeSight|||||| -|Subaru|Impreza 2020-21|EyeSight|||||| -|Toyota|Avalon 2016-18|TSS-P|[3](#footnotes)||||| +|Toyota|Avalon 2016-18|TSS-P|[3](#footnotes)||||| +|Toyota|Avalon Hybrid 2019-21|TSS-P|[3](#footnotes)||||| |Toyota|C-HR 2017-21|All|||||| -|Toyota|C-HR Hybrid 2017-19|All|||||| +|Toyota|C-HR Hybrid 2017-19|All|||||| |Toyota|Corolla 2017-19|All|[3](#footnotes)||||| |Toyota|Prius v 2017|TSS-P|[3](#footnotes)||||| |Toyota|RAV4 2016-18|TSS-P|[3](#footnotes)||||| -|Volkswagen|Atlas 2018-19, 2022[7](#footnotes)|Driver Assistance|||||| +|Toyota|RAV4 2022|All|||||| +|Volkswagen|Arteon 2018, 2021[7](#footnotes)|Driver Assistance|||||| |Volkswagen|California 2021[7](#footnotes)|Driver Assistance|||||| |Volkswagen|Caravelle 2020[7](#footnotes)|Driver Assistance|||||| |Volkswagen|Jetta 2018-21|Driver Assistance|||||| |Volkswagen|Jetta GLI 2021|Driver Assistance|||||| +|Volkswagen|Passat 2015-19[6](#footnotes)|Driver Assistance|||||| |Volkswagen|Tiguan 2019-22[7](#footnotes)|Driver Assistance|||||| diff --git a/release/files_common b/release/files_common index ceb0110606..5230511ddd 100644 --- a/release/files_common +++ b/release/files_common @@ -104,6 +104,7 @@ selfdrive/car/fw_versions.py selfdrive/car/isotp_parallel_query.py selfdrive/car/tests/__init__.py selfdrive/car/tests/test_car_interfaces.py +selfdrive/car/torque_data.json selfdrive/car/body/*.py selfdrive/car/chrysler/*.py diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index e47b085533..55672dd4ab 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -23,14 +23,6 @@ class CarInterface(CarInterfaceBase): ret.steerRateCost = 0.7 ret.steerLimitTimer = 0.4 - # set max lateral acceleration - if candidate in (CAR.PACIFICA_2018, CAR.PACIFICA_2019_HYBRID, CAR.JEEP_CHEROKEE, CAR.JEEP_CHEROKEE_2019): - ret.maxLateralAccel = 1.6 - if candidate in (CAR.PACIFICA_2018_HYBRID,): - ret.maxLateralAccel = 1.4 - if candidate in (CAR.PACIFICA_2017_HYBRID,): - ret.maxLateralAccel = 1.2 - if candidate in (CAR.JEEP_CHEROKEE, CAR.JEEP_CHEROKEE_2019): ret.wheelbase = 2.91 # in meters ret.steerRatio = 12.7 diff --git a/selfdrive/car/docs_definitions.py b/selfdrive/car/docs_definitions.py index e3b46c29e4..3700a24835 100644 --- a/selfdrive/car/docs_definitions.py +++ b/selfdrive/car/docs_definitions.py @@ -7,7 +7,7 @@ from enum import Enum from typing import Dict, List, Optional, Union, no_type_check TACO_TORQUE_THRESHOLD = 2.5 # m/s^2 -GREAT_TORQUE_THRESHOLD = 1.5 # m/s^2 +GREAT_TORQUE_THRESHOLD = 1.4 # m/s^2 GOOD_TORQUE_THRESHOLD = 1.0 # m/s^2 diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index bc43086586..498f4b3347 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -83,7 +83,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 17.7 # Stock 15.7, LiveParameters tire_stiffness_factor = 0.469 # Stock Michelin Energy Saver A/S, LiveParameters ret.centerToFront = ret.wheelbase * 0.45 # Volt Gen 1, TODO corner weigh - ret.maxLateralAccel = 1.6 ret.lateralTuning.pid.kpBP = [0., 40.] ret.lateralTuning.pid.kpV = [0., 0.17] @@ -111,7 +110,6 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.86 ret.steerRatio = 14.4 # end to end is 13.46 ret.centerToFront = ret.wheelbase * 0.4 - ret.maxLateralAccel = 1.4 ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_acadia() elif candidate == CAR.BUICK_REGAL: diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index de5db98ea6..ea7deab5d7 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -100,7 +100,6 @@ class CarInterface(CarInterfaceBase): ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 8000], [0, 2560, 3840]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.1]] else: - ret.maxLateralAccel = 0.4 ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.1], [0.33]] tire_stiffness_factor = 1. @@ -113,7 +112,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 15.38 # 10.93 is end-to-end spec ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 1. - ret.maxLateralAccel = 1.6 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] elif candidate in (CAR.ACCORD, CAR.ACCORDH): @@ -128,7 +126,6 @@ class CarInterface(CarInterfaceBase): if eps_modified: ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.09]] else: - ret.maxLateralAccel = 1.6 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] elif candidate == CAR.ACURA_ILX: @@ -165,7 +162,6 @@ class CarInterface(CarInterfaceBase): ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 10000], [0, 2560, 3840]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.21], [0.07]] else: - ret.maxLateralAccel = 1.7 ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.64], [0.192]] tire_stiffness_factor = 0.677 @@ -190,7 +186,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 13.06 ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.75 - ret.maxLateralAccel = 1.7 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]] elif candidate == CAR.FREED: @@ -222,7 +217,6 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.38 ret.steerRatio = 15.0 # as spec tire_stiffness_factor = 0.444 - ret.maxLateralAccel = 0.9 ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] @@ -232,7 +226,6 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.75 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 11.95 # as spec - ret.maxLateralAccel = 1.2 ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.06]] tire_stiffness_factor = 0.677 @@ -245,7 +238,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 14.35 # as spec ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.82 - ret.maxLateralAccel = 0.8 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]] elif candidate == CAR.ODYSSEY_CHN: @@ -266,7 +258,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 17.25 # as spec ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.444 - ret.maxLateralAccel = 1.5 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] elif candidate == CAR.RIDGELINE: @@ -277,7 +268,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 15.59 # as spec ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.444 - ret.maxLateralAccel = 1.3 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] elif candidate == CAR.INSIGHT: @@ -288,7 +278,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 15.0 # 12.58 is spec end-to-end ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.82 - ret.maxLateralAccel = 1.4 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] elif candidate == CAR.HONDA_E: diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 831b318d55..08af654996 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -63,7 +63,6 @@ class CarInterface(CarInterfaceBase): # Values from optimizer ret.steerRatio = 16.55 # 13.8 is spec end-to-end tire_stiffness_factor = 0.82 - ret.maxLateralAccel = 3.2 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[9., 22.], [9., 22.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.35], [0.05, 0.09]] elif candidate in (CAR.SONATA, CAR.SONATA_HYBRID): @@ -72,7 +71,6 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.84 ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable tire_stiffness_factor = 0.65 - ret.maxLateralAccel = 2.5 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.SONATA_LF: @@ -80,7 +78,6 @@ class CarInterface(CarInterfaceBase): ret.mass = 4497. * CV.LB_TO_KG ret.wheelbase = 2.804 ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable - ret.maxLateralAccel = 1.8 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.PALISADE: @@ -89,7 +86,6 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.90 ret.steerRatio = 15.6 * 1.15 tire_stiffness_factor = 0.63 - ret.maxLateralAccel = 2.5 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]] elif candidate in (CAR.ELANTRA, CAR.ELANTRA_GT_I30): @@ -146,7 +142,6 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.7 ret.steerRatio = 13.73 # Spec tire_stiffness_factor = 0.385 - ret.maxLateralAccel = 3.0 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] if candidate not in (CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.IONIQ_HEV_2022): @@ -196,7 +191,6 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.7 ret.steerRatio = 13.9 if CAR.KIA_NIRO_HEV_2021 else 13.73 # Spec tire_stiffness_factor = 0.385 - ret.maxLateralAccel = 2.9 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] if candidate == CAR.KIA_NIRO_HEV: @@ -228,7 +222,6 @@ class CarInterface(CarInterfaceBase): ret.mass = 1825. + STD_CARGO_KG ret.wheelbase = 2.78 ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable - ret.maxLateralAccel = 2.4 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.KIA_FORTE: @@ -254,7 +247,6 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.85 ret.steerRatio = 13.27 # 2021 Kia K5 Steering Ratio (all trims) tire_stiffness_factor = 0.5 - ret.maxLateralAccel = 2.1 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.KIA_EV6: diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index a2c11bf023..9220aee522 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -1,3 +1,4 @@ +import json import os import time from abc import abstractmethod, ABC @@ -19,6 +20,7 @@ EventName = car.CarEvent.EventName MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS ACCEL_MAX = 2.0 ACCEL_MIN = -3.5 +TORQUE_PARAMS_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data.json') # generic car and radar interfaces @@ -81,7 +83,7 @@ class CarInterfaceBase(ABC): ret.steerControlType = car.CarParams.SteerControlType.torque ret.minSteerSpeed = 0. ret.wheelSpeedFactor = 1.0 - ret.maxLateralAccel = float('nan') + ret.maxLateralAccel = CarInterfaceBase.get_torque_params(candidate)['MAX_LAT_ACCEL_MEASURED'] ret.pcmCruise = True # openpilot's state is tied to the PCM's cruise state on most cars ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this @@ -105,6 +107,12 @@ class CarInterfaceBase(ABC): ret.steerLimitTimer = 1.0 return ret + @staticmethod + def get_torque_params(candidate, default=float('NaN')): + with open(TORQUE_PARAMS_PATH) as f: + data = json.load(f) + return {key: data[key].get(candidate, default) for key in data} + @abstractmethod def _update(self, c: car.CarControl) -> car.CarState: pass diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index edd26a4449..d0d8e91ce1 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -51,7 +51,6 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.5 ret.steerRatio = 17 # learned, 14 stock ret.steerActuatorDelay = 0.1 - ret.maxLateralAccel = 1.3 ret.lateralTuning.pid.kf = 0.00005 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.045, 0.042, 0.20], [0.04, 0.035, 0.045]] @@ -62,7 +61,6 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.5 ret.steerRatio = 17 # learned, 14 stock ret.steerActuatorDelay = 0.1 - ret.maxLateralAccel = 3.2 ret.lateralTuning.pid.kf = 0.000038 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01, 0.065, 0.2], [0.001, 0.015, 0.025]] diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 92024ab0c2..52c89440c7 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +import math import unittest import importlib from parameterized import parameterized @@ -39,7 +40,9 @@ class TestCarInterfaces(unittest.TestCase): if tuning == 'pid': self.assertTrue(len(car_params.lateralTuning.pid.kpV)) elif tuning == 'torque': - self.assertTrue(car_params.lateralTuning.torque.kf > 0) + kf = car_params.lateralTuning.torque.kf + self.assertTrue(not math.isnan(kf) and kf > 0) + self.assertTrue(not math.isnan(car_params.lateralTuning.torque.friction)) elif tuning == 'indi': self.assertTrue(len(car_params.lateralTuning.indi.outerLoopGainV)) diff --git a/selfdrive/car/tests/test_docs.py b/selfdrive/car/tests/test_docs.py index fed6989d40..b4bc14ef00 100755 --- a/selfdrive/car/tests/test_docs.py +++ b/selfdrive/car/tests/test_docs.py @@ -3,6 +3,7 @@ import unittest from selfdrive.car.car_helpers import interfaces, get_interface_attr from selfdrive.car.docs import CARS_MD_OUT, CARS_MD_TEMPLATE, generate_cars_md, get_all_car_info +from selfdrive.car.docs_definitions import Column, Star class TestCarDocs(unittest.TestCase): @@ -37,6 +38,11 @@ class TestCarDocs(unittest.TestCase): if "rav4" in tokens: self.assertIn("RAV4", car.model, "Use correct capitalization") + def test_torque_star(self): + for car in self.all_cars: + if car.car_name == "honda": + self.assertTrue(car.row[Column.STEERING_TORQUE] in (Star.EMPTY, Star.HALF), f"{car.name} has full torque star") + if __name__ == "__main__": unittest.main() diff --git a/selfdrive/car/torque_data.json b/selfdrive/car/torque_data.json new file mode 100644 index 0000000000..63ee0e9bcd --- /dev/null +++ b/selfdrive/car/torque_data.json @@ -0,0 +1 @@ +{"LAT_ACCEL_FACTOR": {"HONDA PILOT 2017": 1.682289482065265, "HONDA CIVIC 2016": 1.5248128495527884, "TOYOTA CAMRY 2018": 2.1115709806216447, "TOYOTA COROLLA HYBRID TSS2 2019": 2.3250600977240077, "TOYOTA RAV4 2019": 2.625504029066767, "HYUNDAI PALISADE 2020": 2.5250855675875634, "TOYOTA SIENNA 2018": 1.8254254785341577, "ACURA RDX 2020": 1.3998101622214894, "TOYOTA RAV4 2017": 1.948190869577896, "HONDA RIDGELINE 2017": 1.4158181862793415, "TOYOTA PRIUS 2017": 1.9142926195557595, "TOYOTA HIGHLANDER HYBRID 2020": 2.1097056247344392, "HYUNDAI SONATA 2020": 3.2488989629905944, "KIA STINGER GT2 2018": 2.7592622336517834, "TOYOTA HIGHLANDER 2020": 2.0408544157877055, "HONDA ACCORD 2018": 1.6374118241564064, "TOYOTA PRIUS TSS2 2021": 2.3207270770298365, "NISSAN LEAF 2018": NaN, "CHRYSLER PACIFICA HYBRID 2019": 1.46050785084946, "LEXUS NX 2020": 2.29533657249232, "TOYOTA RAV4 HYBRID 2019": 2.4003012079562085, "HONDA CIVIC (BOSCH) 2019": 1.6523031416671652, "KIA NIRO HYBRID 2021": 2.743464625803003, "HONDA ACCORD HYBRID 2018": 1.5904016830979033, "LEXUS NX HYBRID 2018": 2.398678119681945, "TOYOTA COROLLA TSS2 2019": 2.3859244449846466, "VOLKSWAGEN ARTEON 1ST GEN": 1.4249208219414902, "TOYOTA CAMRY HYBRID 2021": 2.5434553806317055, "VOLKSWAGEN JETTA 7TH GEN": 1.2228130240634283, "HONDA INSIGHT 2019": 1.468352089969897, "SUBARU FORESTER 2019": 3.6185035528523546, "HYUNDAI ELANTRA 2021": 3.5294999663335185, "HYUNDAI IONIQ ELECTRIC LIMITED 2019": 2.2179616966432905, "HYUNDAI KONA HYBRID 2020": 4.493208192966529, "HONDA ODYSSEY 2018": 1.8838175399087222, "LEXUS RX 2016": 1.3912132245094184, "TOYOTA COROLLA 2017": 3.0143547548384735, "LEXUS ES 2019": 2.012201253045193, "HYUNDAI SANTA FE 2019": 3.039728566484244, "TOYOTA AVALON 2022": 2.4619858654670885, "JEEP GRAND CHEROKEE V6 2018": 1.8411674990629987, "CHEVROLET VOLT PREMIER 2017": 1.5943438675127841, "TOYOTA RAV4 HYBRID 2017": 1.9803053616868995, "LEXUS RX 2020": 1.664616846377383, "TOYOTA HIGHLANDER HYBRID 2018": 1.8866764457400844, "TOYOTA CAMRY HYBRID 2018": 2.014213351947917, "TESLA AP2 MODEL S": NaN, "VOLKSWAGEN GOLF 7TH GEN": 1.4428896585442685, "TOYOTA MIRAI 2021": 2.7217623852898853, "LEXUS IS 2018": 3.5624668608596837, "TOYOTA HIGHLANDER 2017": 1.9199133105823853, "HYUNDAI SONATA HYBRID 2021": 2.7313907441569554, "VOLKSWAGEN ATLAS 1ST GEN": 1.4483948408160645, "LEXUS ES HYBRID 2019": 2.4138026617523547, "HYUNDAI GENESIS 2015-2016": 1.7636839808044658, "JEEP GRAND CHEROKEE 2019": 1.787264083939164, "SUBARU ASCENT LIMITED 2019": 3.0494069339774565, "HONDA CR-V 2017": 1.9828470679233807, "HONDA FIT 2018": 1.594940026552055, "TOYOTA CAMRY 2021": 2.5057990840460342, "AUDI Q3 2ND GEN": 1.4558300885316715, "AUDI A3 3RD GEN": 1.5304173783542625, "LEXUS RX HYBRID 2017": 1.577216425446677, "HONDA CIVIC 2022": 2.69252285552613, "GENESIS G70 2018": 3.866842361627636, "CHRYSLER PACIFICA HYBRID 2018": 1.5771851419640903, "VOLKSWAGEN PASSAT 8TH GEN": 1.2985597059739313, "HONDA CR-V 2016": 0.7745984062630755, "HYUNDAI IONIQ PHEV 2020": 2.5696218908589383, "GMC ACADIA DENALI 2018": 1.3310088601868082, "HYUNDAI SONATA 2019": 1.9736552675022665, "TOYOTA AVALON 2019": 1.7245149905226294, "TOYOTA C-HR 2018": 1.5895016960662856, "HONDA CR-V HYBRID 2019": 2.0687746810729193, "CHRYSLER PACIFICA 2020": 1.40536880000744, "HYUNDAI IONIQ ELECTRIC 2020": 3.3220838625838667, "VOLKSWAGEN TIGUAN 2ND GEN": NaN, "LEXUS NX 2018": 1.7753192756242595, "KIA OPTIMA SX 2019 & 2016": 3.12625562280304, "TOYOTA AVALON HYBRID 2019": 1.7681286449373381, "TOYOTA RAV4 HYBRID 2022": 2.5518187542231816, "HONDA PASSPORT 2021": 1.5174924139130355, "KIA K5 2021": 2.482916204106975, "ACURA ILX 2016": 1.5237423964720282, "HYUNDAI IONIQ HYBRID 2017-2019": 2.3723887901632645, "KIA NIRO EV 2020": 2.924651969180446, "SUBARU IMPREZA SPORT 2020": 2.5317689549587694, "CHRYSLER PACIFICA HYBRID 2017": 1.167126725149114, "HYUNDAI KONA ELECTRIC 2019": 4.201092987427836, "HYUNDAI ELANTRA HYBRID 2021": 3.7153193626001926, "HYUNDAI SANTA FE HYBRID 2022": 3.3049230586030545, "CHRYSLER PACIFICA 2018": 1.524867383058782, "NISSAN ROGUE 2019": NaN, "KIA SORENTO GT LINE 2018": 2.5970979517766213, "COMMA BODY": NaN, "NISSAN LEAF 2018 Instrument Cluster": NaN, "LEXUS RX HYBRID 2020": 1.5460982690267173, "MAZDA CX-9 2021": 1.9514800984278198, "HYUNDAI SANTA FE 2022": 3.5354982200434524, "HYUNDAI SANTA FE PlUG-IN HYBRID 2022": 1.8902492836532216, "HONDA HRV 2019": 2.1262957371020352, "TOYOTA AVALON HYBRID 2022": 2.4142150048378683, "SUBARU IMPREZA LIMITED 2019": 1.2203463907025918, "GENESIS G80 2017": 2.4086794443413906, "VOLKSWAGEN TAOS 1ST GEN": 2.0031666974545947, "KIA FORTE E 2018 & GT 2021": 2.022553820222557, "CADILLAC ESCALADE ESV 2016": 1.5522339636408988, "TOYOTA C-HR 2021": 1.6519334844316687, "TOYOTA C-HR HYBRID 2018": 1.3193315010905482}, "MAX_LAT_ACCEL_MEASURED": {"HONDA PILOT 2017": 0.9069354290994807, "HONDA CIVIC 2016": 0.4030275472529351, "TOYOTA CAMRY 2018": 1.686123168195758, "TOYOTA COROLLA HYBRID TSS2 2019": 1.9139332621491167, "TOYOTA RAV4 2019": 2.234047196286479, "HYUNDAI PALISADE 2020": 1.8303582523301922, "TOYOTA SIENNA 2018": 1.4752503435300715, "ACURA RDX 2020": 0.40911581320000334, "TOYOTA RAV4 2017": 1.6622227720995595, "HONDA RIDGELINE 2017": 0.8224685813281227, "TOYOTA PRIUS 2017": 1.4548827870876067, "TOYOTA HIGHLANDER HYBRID 2020": 2.0649784271823037, "HYUNDAI SONATA 2020": 2.243989856570093, "KIA STINGER GT2 2018": 1.9531287107084392, "TOYOTA HIGHLANDER 2020": 1.659381392090836, "HONDA ACCORD 2018": 0.40486739531686267, "TOYOTA PRIUS TSS2 2021": 1.861541601048098, "NISSAN LEAF 2018": NaN, "CHRYSLER PACIFICA HYBRID 2019": 1.1930739374812243, "LEXUS NX 2020": 1.565268724838564, "TOYOTA RAV4 HYBRID 2019": 2.0915384047218426, "HONDA CIVIC (BOSCH) 2019": 0.4062886118517984, "KIA NIRO HYBRID 2021": NaN, "HONDA ACCORD HYBRID 2018": 0.35128914564548286, "LEXUS NX HYBRID 2018": 1.81821359787186, "TOYOTA COROLLA TSS2 2019": 1.911280958056631, "VOLKSWAGEN ARTEON 1ST GEN": 1.2587939472578302, "TOYOTA CAMRY HYBRID 2021": 2.312510643730013, "VOLKSWAGEN JETTA 7TH GEN": 1.232161945396623, "HONDA INSIGHT 2019": 0.5174836462945298, "SUBARU FORESTER 2019": 2.29255993930968, "HYUNDAI ELANTRA 2021": NaN, "HYUNDAI IONIQ ELECTRIC LIMITED 2019": 2.133978602602408, "HYUNDAI KONA HYBRID 2020": NaN, "HONDA ODYSSEY 2018": 0.8254773781363679, "LEXUS RX 2016": 1.0954776820595344, "TOYOTA COROLLA 2017": 2.2012870528168964, "LEXUS ES 2019": 2.069508805495439, "HYUNDAI SANTA FE 2019": 2.3763720477660253, "TOYOTA AVALON 2022": 2.531962323786023, "JEEP GRAND CHEROKEE V6 2018": 1.4193323242487865, "CHEVROLET VOLT PREMIER 2017": 1.8576430337666092, "TOYOTA RAV4 HYBRID 2017": 1.7425797219020926, "LEXUS RX 2020": 1.5118835180227874, "TOYOTA HIGHLANDER HYBRID 2018": 1.6872527654528833, "TOYOTA CAMRY HYBRID 2018": 1.6793468378089895, "TESLA AP2 MODEL S": NaN, "VOLKSWAGEN GOLF 7TH GEN": 1.5614447712441282, "TOYOTA MIRAI 2021": 2.271146483563897, "LEXUS IS 2018": NaN, "TOYOTA HIGHLANDER 2017": 1.6573774863189379, "HYUNDAI SONATA HYBRID 2021": 1.9464120717803253, "VOLKSWAGEN ATLAS 1ST GEN": 1.6867005451451638, "LEXUS ES HYBRID 2019": 1.956450687999482, "HYUNDAI GENESIS 2015-2016": 1.5359761378898085, "JEEP GRAND CHEROKEE 2019": 1.2418961305308847, "SUBARU ASCENT LIMITED 2019": NaN, "HONDA CR-V 2017": 0.2642062271814174, "HONDA FIT 2018": 0.5896345937094754, "TOYOTA CAMRY 2021": 2.1783533980215166, "AUDI Q3 2ND GEN": 1.1582239457022647, "AUDI A3 3RD GEN": 1.598699116126939, "LEXUS RX HYBRID 2017": 1.319771127672888, "HONDA CIVIC 2022": 1.1806949852580793, "GENESIS G70 2018": 2.2496820850331134, "CHRYSLER PACIFICA HYBRID 2018": 1.294798200968084, "VOLKSWAGEN PASSAT 8TH GEN": 1.247540921731637, "HONDA CR-V 2016": 0.6991119250342539, "HYUNDAI IONIQ PHEV 2020": 1.9062392690595655, "GMC ACADIA DENALI 2018": 1.2986994230652662, "HYUNDAI SONATA 2019": 1.257445187146704, "TOYOTA AVALON 2019": 1.664577368475227, "TOYOTA C-HR 2018": 1.308490445144888, "HONDA CR-V HYBRID 2019": 0.4693072746041504, "CHRYSLER PACIFICA 2020": 1.1712413003138664, "HYUNDAI IONIQ ELECTRIC 2020": NaN, "VOLKSWAGEN TIGUAN 2ND GEN": 1.1573057001955744, "LEXUS NX 2018": 1.9457312007432144, "KIA OPTIMA SX 2019 & 2016": 2.0928228595938845, "TOYOTA AVALON HYBRID 2019": NaN, "TOYOTA RAV4 HYBRID 2022": 1.7647290773049569, "HONDA PASSPORT 2021": 0.8248357750132685, "KIA K5 2021": 1.4628018983720577, "ACURA ILX 2016": 0.6330753921140401, "HYUNDAI IONIQ HYBRID 2017-2019": NaN, "KIA NIRO EV 2020": 2.020186575503497, "SUBARU IMPREZA SPORT 2020": 2.136786720514988, "CHRYSLER PACIFICA HYBRID 2017": 1.0642918033307907, "HYUNDAI KONA ELECTRIC 2019": NaN, "HYUNDAI ELANTRA HYBRID 2021": NaN, "HYUNDAI SANTA FE HYBRID 2022": NaN, "CHRYSLER PACIFICA 2018": 1.3654603720349934, "NISSAN ROGUE 2019": NaN, "KIA SORENTO GT LINE 2018": NaN, "COMMA BODY": NaN, "NISSAN LEAF 2018 Instrument Cluster": NaN, "LEXUS RX HYBRID 2020": 1.255230465866663, "MAZDA CX-9 2021": NaN, "HYUNDAI SANTA FE 2022": 3.3823387508235827, "HYUNDAI SANTA FE PlUG-IN HYBRID 2022": 1.544104124172169, "HONDA HRV 2019": 0.7492792210307291, "TOYOTA AVALON HYBRID 2022": NaN, "SUBARU IMPREZA LIMITED 2019": 0.8127509604734238, "GENESIS G80 2017": NaN, "VOLKSWAGEN TAOS 1ST GEN": 1.6590543949912684, "KIA FORTE E 2018 & GT 2021": 2.3970573789339786, "CADILLAC ESCALADE ESV 2016": NaN, "TOYOTA C-HR 2021": 1.3559230155096402, "TOYOTA C-HR HYBRID 2018": 0.8910235787356033}, "FRICTION": {"HONDA PILOT 2017": 0.2168217463499328, "HONDA CIVIC 2016": 0.28406761310944795, "TOYOTA CAMRY 2018": 0.1327947477896041, "TOYOTA COROLLA HYBRID TSS2 2019": 0.21792021497538405, "TOYOTA RAV4 2019": 0.12757022360707945, "HYUNDAI PALISADE 2020": 0.13391574986922777, "TOYOTA SIENNA 2018": 0.1853443239485906, "ACURA RDX 2020": 0.18058553315570297, "TOYOTA RAV4 2017": 0.14319170324556796, "HONDA RIDGELINE 2017": 0.2380553573913589, "TOYOTA PRIUS 2017": 0.2079869382946584, "TOYOTA HIGHLANDER HYBRID 2020": 0.14038812589302646, "HYUNDAI SONATA 2020": 0.08266051305053168, "KIA STINGER GT2 2018": 0.11909534626930472, "TOYOTA HIGHLANDER 2020": 0.14658637853444048, "HONDA ACCORD 2018": 0.21616610462729247, "TOYOTA PRIUS TSS2 2021": 0.20613763260512002, "NISSAN LEAF 2018": NaN, "CHRYSLER PACIFICA HYBRID 2019": 0.16250373743651828, "LEXUS NX 2020": 0.14404022591302845, "TOYOTA RAV4 HYBRID 2019": 0.1319247989758836, "HONDA CIVIC (BOSCH) 2019": 0.2575217845562353, "KIA NIRO HYBRID 2021": 0.14468633728800306, "HONDA ACCORD HYBRID 2018": 0.21150723931119184, "LEXUS NX HYBRID 2018": 0.16117151597250162, "TOYOTA COROLLA TSS2 2019": 0.21045927995242877, "VOLKSWAGEN ARTEON 1ST GEN": 0.17828895368353925, "TOYOTA CAMRY HYBRID 2021": 0.16283734136957057, "VOLKSWAGEN JETTA 7TH GEN": 0.19508489725001105, "HONDA INSIGHT 2019": 0.25750800088299297, "SUBARU FORESTER 2019": 0.11783702069698135, "HYUNDAI ELANTRA 2021": 0.09377564130711125, "HYUNDAI IONIQ ELECTRIC LIMITED 2019": 0.14740189509875762, "HYUNDAI KONA HYBRID 2020": 0.0863709736632968, "HONDA ODYSSEY 2018": 0.2125595696498247, "LEXUS RX 2016": 0.21475140622981923, "TOYOTA COROLLA 2017": 0.12325064090161544, "LEXUS ES 2019": 0.12757526660498053, "HYUNDAI SANTA FE 2019": 0.12230125806479573, "TOYOTA AVALON 2022": 0.11030226705639488, "JEEP GRAND CHEROKEE V6 2018": 0.12871972792344108, "CHEVROLET VOLT PREMIER 2017": 0.16697256960295873, "TOYOTA RAV4 HYBRID 2017": 0.14074453855329072, "LEXUS RX 2020": 0.2249895411716623, "TOYOTA HIGHLANDER HYBRID 2018": 0.16692807938039034, "TOYOTA CAMRY HYBRID 2018": 0.13418904852016877, "TESLA AP2 MODEL S": NaN, "VOLKSWAGEN GOLF 7TH GEN": 0.19324413131475543, "TOYOTA MIRAI 2021": 0.20035237756713503, "LEXUS IS 2018": 0.073103111226694, "TOYOTA HIGHLANDER 2017": 0.17502689439420385, "HYUNDAI SONATA HYBRID 2021": 0.09518615688045734, "VOLKSWAGEN ATLAS 1ST GEN": 0.12761803335799474, "LEXUS ES HYBRID 2019": 0.1682771025433274, "HYUNDAI GENESIS 2015-2016": 0.10254237048034251, "JEEP GRAND CHEROKEE 2019": 0.15702739382013717, "SUBARU ASCENT LIMITED 2019": 0.12936982863095342, "HONDA CR-V 2017": 0.22518506713451308, "HONDA FIT 2018": 0.10803295063463647, "TOYOTA CAMRY 2021": 0.15512845523424743, "AUDI Q3 2ND GEN": 0.14083949977629878, "AUDI A3 3RD GEN": 0.1611945965384188, "LEXUS RX HYBRID 2017": 0.19322020114452776, "HONDA CIVIC 2022": 0.24279247053469405, "GENESIS G70 2018": 0.06869638264150804, "CHRYSLER PACIFICA HYBRID 2018": 0.13887505891474383, "VOLKSWAGEN PASSAT 8TH GEN": 0.21714039653367842, "HONDA CR-V 2016": 0.41726236462791455, "HYUNDAI IONIQ PHEV 2020": 0.13800461817330806, "GMC ACADIA DENALI 2018": 0.3447163106452739, "HYUNDAI SONATA 2019": 0.15371520337813344, "TOYOTA AVALON 2019": 0.10392921606262978, "TOYOTA C-HR 2018": 0.2015190716953846, "HONDA CR-V HYBRID 2019": 0.19595630321202379, "CHRYSLER PACIFICA 2020": 0.14337114313208268, "HYUNDAI IONIQ ELECTRIC 2020": 0.08104502306679212, "VOLKSWAGEN TIGUAN 2ND GEN": NaN, "LEXUS NX 2018": 0.1471001686544422, "KIA OPTIMA SX 2019 & 2016": 0.11703652166984638, "TOYOTA AVALON HYBRID 2019": 0.10863628402866225, "TOYOTA RAV4 HYBRID 2022": 0.14334213238415072, "HONDA PASSPORT 2021": 0.19826160782809032, "KIA K5 2021": 0.1027179720106188, "ACURA ILX 2016": 0.35663988815912573, "HYUNDAI IONIQ HYBRID 2017-2019": 0.12332151728479951, "KIA NIRO EV 2020": 0.0892074288578785, "SUBARU IMPREZA SPORT 2020": 0.15841234487251604, "CHRYSLER PACIFICA HYBRID 2017": 0.1345638758810282, "HYUNDAI KONA ELECTRIC 2019": 0.08503096350356723, "HYUNDAI ELANTRA HYBRID 2021": 0.09887804390243872, "HYUNDAI SANTA FE HYBRID 2022": 0.11171499761140577, "CHRYSLER PACIFICA 2018": 0.13611561752951415, "NISSAN ROGUE 2019": NaN, "KIA SORENTO GT LINE 2018": 0.10502695501512567, "COMMA BODY": NaN, "NISSAN LEAF 2018 Instrument Cluster": NaN, "LEXUS RX HYBRID 2020": 0.21818156330777305, "MAZDA CX-9 2021": 0.1793735649504697, "HYUNDAI SANTA FE 2022": 0.09184808719698756, "HYUNDAI SANTA FE PlUG-IN HYBRID 2022": 0.14050744688135813, "HONDA HRV 2019": 0.17840321248608593, "TOYOTA AVALON HYBRID 2022": 0.16159049452515487, "SUBARU IMPREZA LIMITED 2019": 0.20322553080306893, "GENESIS G80 2017": 0.07934444681782107, "VOLKSWAGEN TAOS 1ST GEN": 0.18276122764341485, "KIA FORTE E 2018 & GT 2021": 0.11406160665068436, "CADILLAC ESCALADE ESV 2016": 0.15063766975884627, "TOYOTA C-HR 2021": 0.22798633346500694, "TOYOTA C-HR HYBRID 2018": 0.2036375866375624}, "ERROR_RATIO": {"HONDA PILOT 2017": 0.6158457247286419, "HONDA CIVIC 2016": 2.0785618623350928, "TOYOTA CAMRY 2018": 0.17356565057429169, "TOYOTA COROLLA HYBRID TSS2 2019": 0.10094741777075293, "TOYOTA RAV4 2019": 0.11812042718338775, "HYUNDAI PALISADE 2020": 0.30639442561268304, "TOYOTA SIENNA 2018": 0.1117307389748361, "ACURA RDX 2020": 1.9801454495960717, "TOYOTA RAV4 2017": 0.08589486116378196, "HONDA RIDGELINE 2017": 0.4319851914417577, "TOYOTA PRIUS 2017": 0.17281316158588575, "TOYOTA HIGHLANDER HYBRID 2020": 0.046325388721577, "HYUNDAI SONATA 2020": 0.4109860794021653, "KIA STINGER GT2 2018": 0.3517628781488943, "TOYOTA HIGHLANDER 2020": 0.14155072865224166, "HONDA ACCORD 2018": 2.510398061115294, "TOYOTA PRIUS TSS2 2021": 0.13593456264106363, "NISSAN LEAF 2018": NaN, "CHRYSLER PACIFICA HYBRID 2019": 0.08794943266738546, "LEXUS NX 2020": 0.3743942573190866, "TOYOTA RAV4 HYBRID 2019": 0.0845492503791727, "HONDA CIVIC (BOSCH) 2019": 2.4329816697390063, "KIA NIRO HYBRID 2021": NaN, "HONDA ACCORD HYBRID 2018": 2.9252406767451804, "LEXUS NX HYBRID 2018": 0.23060712246809048, "TOYOTA COROLLA TSS2 2019": 0.13822363784977285, "VOLKSWAGEN ARTEON 1ST GEN": 0.009661691674299285, "TOYOTA CAMRY HYBRID 2021": 0.029451711159377333, "VOLKSWAGEN JETTA 7TH GEN": 0.16591473170144055, "HONDA INSIGHT 2019": 1.3398692842898896, "SUBARU FORESTER 2019": 0.5269683780697442, "HYUNDAI ELANTRA 2021": NaN, "HYUNDAI IONIQ ELECTRIC LIMITED 2019": 0.02971857401969039, "HYUNDAI KONA HYBRID 2020": NaN, "HONDA ODYSSEY 2018": 1.0245957242729038, "LEXUS RX 2016": 0.07392586589971588, "TOYOTA COROLLA 2017": 0.31336988069649124, "LEXUS ES 2019": 0.08933657038050916, "HYUNDAI SANTA FE 2019": 0.2276812089092099, "TOYOTA AVALON 2022": 0.07120118798045925, "JEEP GRAND CHEROKEE V6 2018": 0.2065164316228118, "CHEVROLET VOLT PREMIER 2017": 0.2316223989408518, "TOYOTA RAV4 HYBRID 2017": 0.055653752888652736, "LEXUS RX 2020": 0.047792182371008345, "TOYOTA HIGHLANDER HYBRID 2018": 0.019259474082467202, "TOYOTA CAMRY HYBRID 2018": 0.11949733140330816, "TESLA AP2 MODEL S": NaN, "VOLKSWAGEN GOLF 7TH GEN": 0.1996863736436734, "TOYOTA MIRAI 2021": 0.11019259478417197, "LEXUS IS 2018": NaN, "TOYOTA HIGHLANDER 2017": 0.05279963713251727, "HYUNDAI SONATA HYBRID 2021": 0.3543918194389536, "VOLKSWAGEN ATLAS 1ST GEN": 0.21694647502209782, "LEXUS ES HYBRID 2019": 0.14775474433507507, "HYUNDAI GENESIS 2015-2016": 0.0814892037361157, "JEEP GRAND CHEROKEE 2019": 0.3126997097753535, "SUBARU ASCENT LIMITED 2019": NaN, "HONDA CR-V 2017": 5.652613829506629, "HONDA FIT 2018": 1.5217432826711779, "TOYOTA CAMRY 2021": 0.07910435053686729, "AUDI Q3 2ND GEN": 0.13535089102138698, "AUDI A3 3RD GEN": 0.14353941401245793, "LEXUS RX HYBRID 2017": 0.048663813961824696, "HONDA CIVIC 2022": 1.0748206908458815, "GENESIS G70 2018": 0.688303429295532, "CHRYSLER PACIFICA HYBRID 2018": 0.11083725786301112, "VOLKSWAGEN PASSAT 8TH GEN": 0.13315924904555493, "HONDA CR-V 2016": 0.488871482749128, "HYUNDAI IONIQ PHEV 2020": 0.2756096845519595, "GMC ACADIA DENALI 2018": 0.24055364003040136, "HYUNDAI SONATA 2019": 0.4473315280277132, "TOYOTA AVALON 2019": 0.026428086100632363, "TOYOTA C-HR 2018": 0.06075105822970755, "HONDA CR-V HYBRID 2019": 2.9906016360828276, "CHRYSLER PACIFICA 2020": 0.07748732608487266, "HYUNDAI IONIQ ELECTRIC 2020": NaN, "VOLKSWAGEN TIGUAN 2ND GEN": NaN, "LEXUS NX 2018": 0.16318394527060903, "KIA OPTIMA SX 2019 & 2016": 0.4378756841929454, "TOYOTA AVALON HYBRID 2019": NaN, "TOYOTA RAV4 HYBRID 2022": 0.36478548056633514, "HONDA PASSPORT 2021": 0.5993860184637646, "KIA K5 2021": 0.6271500841947655, "ACURA ILX 2016": 0.8435442647921855, "HYUNDAI IONIQ HYBRID 2017-2019": NaN, "KIA NIRO EV 2020": 0.40355577782011604, "SUBARU IMPREZA SPORT 2020": 0.11071291640854522, "CHRYSLER PACIFICA HYBRID 2017": 0.029812269495458284, "HYUNDAI KONA ELECTRIC 2019": NaN, "HYUNDAI ELANTRA HYBRID 2021": NaN, "HYUNDAI SANTA FE HYBRID 2022": NaN, "CHRYSLER PACIFICA 2018": 0.01705753895996445, "NISSAN ROGUE 2019": NaN, "KIA SORENTO GT LINE 2018": NaN, "COMMA BODY": NaN, "NISSAN LEAF 2018 Instrument Cluster": NaN, "LEXUS RX HYBRID 2020": 0.05790668871480552, "MAZDA CX-9 2021": NaN, "HYUNDAI SANTA FE 2022": 0.018126919430513307, "HYUNDAI SANTA FE PlUG-IN HYBRID 2022": 0.1331760659016062, "HONDA HRV 2019": 1.599688433820939, "TOYOTA AVALON HYBRID 2022": NaN, "SUBARU IMPREZA LIMITED 2019": 0.2514545160390271, "GENESIS G80 2017": NaN, "VOLKSWAGEN TAOS 1ST GEN": 0.09725484306423876, "KIA FORTE E 2018 & GT 2021": 0.20381871942480628, "CADILLAC ESCALADE ESV 2016": NaN, "TOYOTA C-HR 2021": 0.05016813984196128, "TOYOTA C-HR HYBRID 2018": 0.2521485862766935}} \ No newline at end of file diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 9440c73f5d..e737b4d2e7 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -74,7 +74,6 @@ class CarInterface(CarInterfaceBase): ret.wheelSpeedFactor = 1.035 tire_stiffness_factor = 0.5533 ret.mass = 4481. * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max - ret.maxLateralAccel = 1.4 set_lat_tune(ret.lateralTuning, LatTunes.PID_C) elif candidate in (CAR.CHR, CAR.CHRH): @@ -83,7 +82,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 13.6 tire_stiffness_factor = 0.7933 ret.mass = 3300. * CV.LB_TO_KG + STD_CARGO_KG - ret.maxLateralAccel = 1.3 set_lat_tune(ret.lateralTuning, LatTunes.PID_F) elif candidate in (CAR.CAMRY, CAR.CAMRYH, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2): @@ -96,7 +94,6 @@ class CarInterface(CarInterfaceBase): ret.maxLateralAccel = 2.4 set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=ret.maxLateralAccel, FRICTION=0.05) else: - ret.maxLateralAccel = 2.0 set_lat_tune(ret.lateralTuning, LatTunes.PID_C) elif candidate in (CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2): @@ -105,7 +102,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 16.0 tire_stiffness_factor = 0.8 ret.mass = 4700. * CV.LB_TO_KG + STD_CARGO_KG # 4260 + 4-5 people - ret.maxLateralAccel = 2.0 set_lat_tune(ret.lateralTuning, LatTunes.PID_G) elif candidate in (CAR.HIGHLANDER, CAR.HIGHLANDERH): @@ -114,7 +110,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 16.0 tire_stiffness_factor = 0.8 ret.mass = 4607. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid limited - ret.maxLateralAccel = 1.8 set_lat_tune(ret.lateralTuning, LatTunes.PID_G) elif candidate in (CAR.AVALON, CAR.AVALON_2019, CAR.AVALONH_2019, CAR.AVALON_TSS2, CAR.AVALONH_TSS2): @@ -125,7 +120,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 14.8 # Found at https://pressroom.toyota.com/releases/2016+avalon+product+specs.download tire_stiffness_factor = 0.7983 ret.mass = 3505. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - ret.maxLateralAccel = 1.6 set_lat_tune(ret.lateralTuning, LatTunes.PID_H) elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022): @@ -134,7 +128,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 14.3 tire_stiffness_factor = 0.7933 ret.mass = 3585. * CV.LB_TO_KG + STD_CARGO_KG # Average between ICE and Hybrid - ret.maxLateralAccel = 2.5 set_lat_tune(ret.lateralTuning, LatTunes.PID_D) # 2019+ RAV4 TSS2 uses two different steering racks and specific tuning seems to be necessary. @@ -159,7 +152,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 16.0 # not optimized tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 3677. * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max - ret.maxLateralAccel = 2.2 set_lat_tune(ret.lateralTuning, LatTunes.PID_D) elif candidate == CAR.SIENNA: @@ -168,7 +160,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 15.5 tire_stiffness_factor = 0.444 ret.mass = 4590. * CV.LB_TO_KG + STD_CARGO_KG - ret.maxLateralAccel = 1.6 set_lat_tune(ret.lateralTuning, LatTunes.PID_J) elif candidate in (CAR.LEXUS_IS, CAR.LEXUS_RC): @@ -192,7 +183,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 14.7 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 4070 * CV.LB_TO_KG + STD_CARGO_KG - ret.maxLateralAccel = 2.0 set_lat_tune(ret.lateralTuning, LatTunes.PID_C) elif candidate == CAR.PRIUS_TSS2: @@ -201,7 +191,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 13.4 # True steerRatio from older prius tire_stiffness_factor = 0.6371 # hand-tune ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG - ret.maxLateralAccel = 2.0 set_lat_tune(ret.lateralTuning, LatTunes.PID_N) elif candidate == CAR.MIRAI: @@ -210,7 +199,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 14.8 tire_stiffness_factor = 0.8 ret.mass = 4300. * CV.LB_TO_KG + STD_CARGO_KG - ret.maxLateralAccel = 2.4 set_lat_tune(ret.lateralTuning, LatTunes.PID_C) elif candidate in (CAR.ALPHARD_TSS2, CAR.ALPHARDH_TSS2): diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 90bbd6d889..3a0d7c8ce8 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -64,17 +64,14 @@ class CarInterface(CarInterfaceBase): elif candidate == CAR.ATLAS_MK1: ret.mass = 2011 + STD_CARGO_KG ret.wheelbase = 2.98 - ret.maxLateralAccel = 1.4 elif candidate == CAR.GOLF_MK7: ret.mass = 1397 + STD_CARGO_KG ret.wheelbase = 2.62 - ret.maxLateralAccel = 1.5 elif candidate == CAR.JETTA_MK7: ret.mass = 1328 + STD_CARGO_KG ret.wheelbase = 2.71 - ret.maxLateralAccel = 1.2 elif candidate == CAR.PASSAT_MK8: ret.mass = 1551 + STD_CARGO_KG @@ -95,7 +92,6 @@ class CarInterface(CarInterfaceBase): elif candidate == CAR.TIGUAN_MK2: ret.mass = 1715 + STD_CARGO_KG ret.wheelbase = 2.74 - ret.maxLateralAccel = 1.1 elif candidate == CAR.TOURAN_MK2: ret.mass = 1516 + STD_CARGO_KG @@ -113,7 +109,6 @@ class CarInterface(CarInterfaceBase): elif candidate == CAR.AUDI_A3_MK3: ret.mass = 1335 + STD_CARGO_KG ret.wheelbase = 2.61 - ret.maxLateralAccel = 1.7 elif candidate == CAR.AUDI_Q2_MK1: ret.mass = 1205 + STD_CARGO_KG @@ -122,7 +117,6 @@ class CarInterface(CarInterfaceBase): elif candidate == CAR.AUDI_Q3_MK2: ret.mass = 1623 + STD_CARGO_KG ret.wheelbase = 2.68 - ret.maxLateralAccel = 1.6 elif candidate == CAR.SEAT_ATECA_MK1: ret.mass = 1900 + STD_CARGO_KG diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 2f42570934..298de7999a 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -f301bdf52b271c8c4ef0f2bce3be9c8f90037652 +935abbc21282e10572c9324382feba3fee2fe379 \ No newline at end of file From f398b3efc92f59f407379d38ead0f99bf814b95c Mon Sep 17 00:00:00 2001 From: AlexandreSato <66435071+AlexandreSato@users.noreply.github.com> Date: Fri, 10 Jun 2022 22:45:32 -0300 Subject: [PATCH 37/93] Fix Lexus NX Hybrid 2020 engine ecu (#24817) Wrong address in engine ecu. --- selfdrive/car/toyota/values.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 7e6cfa0d09..2c151266cf 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -1585,7 +1585,7 @@ FW_VERSIONS = { ], }, CAR.LEXUS_NXH_TSS2: { - (Ecu.engine, 0x700, None): [ + (Ecu.engine, 0x7e0, None): [ b'\x0237887000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ From 88a100435f1da9bf061b360e8a5cd02e414d6e8b Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 10 Jun 2022 22:52:34 -0700 Subject: [PATCH 38/93] compatibility docs: auto-generate star descriptions (#24809) * Auto-generate star descriptions * Need this for the website * And this * required changes to make the website generation work * better names * Revert "better names" This reverts commit be7dbbb5d846d7d55a1ad69533945e6a6c8a0b7c. * simpler --- selfdrive/car/CARS_template.md | 29 ++++++++-------------------- selfdrive/car/docs.py | 4 ++-- selfdrive/car/docs_definitions.py | 32 +++++++++++++++++++++++++++++++ 3 files changed, 42 insertions(+), 23 deletions(-) diff --git a/selfdrive/car/CARS_template.md b/selfdrive/car/CARS_template.md index f93dc64b06..891445a558 100644 --- a/selfdrive/car/CARS_template.md +++ b/selfdrive/car/CARS_template.md @@ -14,28 +14,15 @@ Cars are organized into three tiers: How We Rate The Cars --- -### openpilot Adaptive Cruise Control (ACC) -- {{star_icon.format(Star.FULL.value)}} - openpilot is able to control the gas and brakes. -- {{star_icon.format(Star.HALF.value)}} - openpilot is able to control the gas and brakes with some restrictions. -- {{star_icon.format(Star.EMPTY.value)}} - The gas and brakes are controlled by the car's stock Adaptive Cruise Control (ACC) system. - -### Stop and Go -- {{star_icon.format(Star.FULL.value)}} - Adaptive Cruise Control (ACC) operates down to 0 mph. -- {{star_icon.format(Star.EMPTY.value)}} - Adaptive Cruise Control (ACC) available only above certain speeds. See your car's manual for the minimum speed. - -### Steer to 0 -- {{star_icon.format(Star.FULL.value)}} - openpilot can control the steering wheel down to 0 mph. -- {{star_icon.format(Star.EMPTY.value)}} - No steering control below certain speeds. - -### Steering Torque -- {{star_icon.format(Star.FULL.value)}} - Car has enough steering torque to take tighter turns. -- {{star_icon.format(Star.HALF.value)}} - Car has enough steering torque for comfortable highway driving. -- {{star_icon.format(Star.EMPTY.value)}} - Limited ability to make turns. - -### Actively Maintained -- {{star_icon.format(Star.FULL.value)}} - Mainline software support, harness hardware sold by comma, lots of users, primary development target. -- {{star_icon.format(Star.EMPTY.value)}} - Low user count, community maintained, harness hardware not sold by comma. +{% for star_row in star_descriptions.values() %} +{% for name, stars in star_row.items() %} +### {{name}} +{% for star, description in stars %} +- {{star_icon.format(star)}} - {{description}} +{% endfor %} +{% endfor %} +{% endfor %} **All supported cars can move between the tiers as support changes.** {% for tier, cars in tiers.items() %} diff --git a/selfdrive/car/docs.py b/selfdrive/car/docs.py index 899f2c0878..860503dbdd 100755 --- a/selfdrive/car/docs.py +++ b/selfdrive/car/docs.py @@ -7,7 +7,7 @@ from natsort import natsorted from typing import Dict, List from common.basedir import BASEDIR -from selfdrive.car.docs_definitions import CarInfo, Column, Star, Tier +from selfdrive.car.docs_definitions import STAR_DESCRIPTIONS, CarInfo, Column, Star, Tier from selfdrive.car.car_helpers import interfaces, get_interface_attr from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR as HKG_RADAR_START_ADDR from selfdrive.car.tests.routes import non_tested_cars @@ -65,7 +65,7 @@ def generate_cars_md(all_car_info: List[CarInfo], template_fn: str) -> str: footnotes = [fn.value.text for fn in ALL_FOOTNOTES] cars_md: str = template.render(tiers=sort_by_tier(all_car_info), all_car_info=all_car_info, - footnotes=footnotes, Star=Star, Column=Column) + footnotes=footnotes, Star=Star, Column=Column, star_descriptions=STAR_DESCRIPTIONS) return cars_md diff --git a/selfdrive/car/docs_definitions.py b/selfdrive/car/docs_definitions.py index 3700a24835..f09dad7860 100644 --- a/selfdrive/car/docs_definitions.py +++ b/selfdrive/car/docs_definitions.py @@ -150,3 +150,35 @@ class Harness(Enum): nissan_b = "Nissan B" mazda = "Mazda" none = "None" + + +STAR_DESCRIPTIONS = { + "Gas & Brakes": { # icon and row name + "openpilot Adaptive Cruise Control (ACC)": [ # star column + [Star.FULL.value, "openpilot is able to control the gas and brakes."], + [Star.HALF.value, "openpilot is able to control the gas and brakes with some restrictions."], + [Star.EMPTY.value, "The gas and brakes are controlled by the car's stock Adaptive Cruise Control (ACC) system."], + ], + Column.FSR_LONGITUDINAL.value: [ + [Star.FULL.value, "Adaptive Cruise Control (ACC) operates down to 0 mph."], + [Star.EMPTY.value, "Adaptive Cruise Control (ACC) available only above certain speeds. See your car's manual for the minimum speed."], + ], + }, + "Steering": { + Column.FSR_STEERING.value: [ + [Star.FULL.value, "openpilot can control the steering wheel down to 0 mph."], + [Star.EMPTY.value, "No steering control below certain speeds."], + ], + Column.STEERING_TORQUE.value: [ + [Star.FULL.value, "Car has enough steering torque to take tighter turns."], + [Star.HALF.value, "Car has enough steering torque for comfortable highway driving."], + [Star.EMPTY.value, "Limited ability to make turns."], + ], + }, + "Support": { + Column.MAINTAINED.value: [ + [Star.FULL.value, "Mainline software support, harness hardware sold by comma, lots of users, primary development target."], + [Star.EMPTY.value, "Low user count, community maintained, harness hardware not sold by comma."], + ], + }, +} From 843e59f6f0be33e6c51a35ef8912f5b7c4b58ca5 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Fri, 10 Jun 2022 22:52:48 -0700 Subject: [PATCH 39/93] Misc torque control fixes (#24801) * Fiction compensation should be based on error * Update refs * Add deadzone * update ref --- cereal | 2 +- selfdrive/car/toyota/interface.py | 2 +- selfdrive/car/toyota/tunes.py | 4 ++-- selfdrive/controls/lib/drive_helpers.py | 16 +++++++++++++--- selfdrive/controls/lib/latcontrol_torque.py | 20 +++++++++++--------- selfdrive/controls/lib/longcontrol.py | 12 +----------- selfdrive/test/process_replay/ref_commit | 2 +- 7 files changed, 30 insertions(+), 28 deletions(-) diff --git a/cereal b/cereal index ff49c8e126..a197e38c0c 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit ff49c8e126ea04889af13d690ceaf520ce7084d2 +Subproject commit a197e38c0cef00ececf4c7500438d37659d9199e diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index e737b4d2e7..83eed611c3 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -39,7 +39,7 @@ class CarInterface(CarInterfaceBase): tire_stiffness_factor = 0.6371 # hand-tune ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG ret.maxLateralAccel = 1.7 - set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=ret.maxLateralAccel, FRICTION=0.06) + set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=ret.maxLateralAccel, FRICTION=0.06, steering_angle_deadzone_deg=1.0) elif candidate == CAR.PRIUS_V: stop_and_go = True diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py index e97ed6b056..3de6daae21 100644 --- a/selfdrive/car/toyota/tunes.py +++ b/selfdrive/car/toyota/tunes.py @@ -50,9 +50,9 @@ def set_long_tune(tune, name): ###### LAT ###### -def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=0.01, use_steering_angle=True): +def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=0.01, steering_angle_deadzone_deg=0.0, use_steering_angle=True): if name == LatTunes.TORQUE: - set_torque_tune(tune, MAX_LAT_ACCEL, FRICTION) + set_torque_tune(tune, MAX_LAT_ACCEL, FRICTION, steering_angle_deadzone_deg) elif 'PID' in str(name): tune.init('pid') tune.pid.kiBP = [0.0] diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 91981259aa..1fecdd7c63 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -38,6 +38,16 @@ class MPC_COST_LAT: STEER_RATE = 1.0 +def apply_deadzone(error, deadzone): + if error > deadzone: + error -= deadzone + elif error < - deadzone: + error += deadzone + else: + error = 0. + return error + + def rate_limit(new_value, last_value, dw_step, up_step): return clip(new_value, last_value + dw_step, last_value + up_step) @@ -98,10 +108,10 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates): curvatures = [0.0]*CONTROL_N curvature_rates = [0.0]*CONTROL_N v_ego = max(v_ego, 0.1) - + # TODO this needs more thought, use .2s extra for now to estimate other delays delay = CP.steerActuatorDelay + .2 - + # MPC can plan to turn the wheel and turn back before t_delay. This means # in high delay cases some corrections never even get commanded. So just use # psi to calculate a simple linearization of desired curvature @@ -109,7 +119,7 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates): psi = interp(delay, T_IDXS[:CONTROL_N], psis) average_curvature_desired = psi / (v_ego * delay) desired_curvature = 2 * average_curvature_desired - current_curvature_desired - + # This is the "desired rate of the setpoint" not an actual desired rate desired_curvature_rate = curvature_rates[0] max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 47f0b69f2b..12714082a6 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -4,6 +4,7 @@ from cereal import log from common.numpy_fast import interp from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED from selfdrive.controls.lib.pid import PIDController +from selfdrive.controls.lib.drive_helpers import apply_deadzone from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY # At higher speeds (25+mph) we can assume: @@ -22,13 +23,14 @@ LOW_SPEED_FACTOR = 200 JERK_THRESHOLD = 0.2 -def set_torque_tune(tune, MAX_LAT_ACCEL=2.5, FRICTION=0.01): +def set_torque_tune(tune, MAX_LAT_ACCEL=2.5, FRICTION=0.01, steering_angle_deadzone_deg=0.0): tune.init('torque') tune.torque.useSteeringAngle = True tune.torque.kp = 1.0 / MAX_LAT_ACCEL tune.torque.kf = 1.0 / MAX_LAT_ACCEL tune.torque.ki = 0.1 / MAX_LAT_ACCEL tune.torque.friction = FRICTION + tune.torque.steeringAngleDeadzoneDeg = steering_angle_deadzone_deg class LatControlTorque(LatControl): @@ -40,10 +42,7 @@ class LatControlTorque(LatControl): self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle self.friction = CP.lateralTuning.torque.friction self.kf = CP.lateralTuning.torque.kf - - def reset(self): - super().reset() - self.pid.reset() + self.steering_angle_deadzone_deg = CP.lateralTuning.torque.steeringAngleDeadzoneDeg def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): pid_log = log.ControlsState.LateralTorqueState.new_message() @@ -54,24 +53,27 @@ class LatControlTorque(LatControl): else: if self.use_steering_angle: actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) + curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) else: actual_curvature_vm = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) actual_curvature_llk = llk.angularVelocityCalibrated.value[2] / CS.vEgo actual_curvature = interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_llk]) + curvature_deadzone = 0.0 desired_lateral_accel = desired_curvature * CS.vEgo ** 2 - + # desired rate is the desired rate of change in the setpoint, not the absolute desired curvature - desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2 + #desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2 actual_lateral_accel = actual_curvature * CS.vEgo ** 2 + lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_curvature measurement = actual_lateral_accel + LOW_SPEED_FACTOR * actual_curvature - error = setpoint - measurement + error = apply_deadzone(setpoint - measurement, lateral_accel_deadzone) pid_log.error = error ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY # convert friction into lateral accel units for feedforward - friction_compensation = interp(desired_lateral_jerk, [-JERK_THRESHOLD, JERK_THRESHOLD], [-self.friction, self.friction]) + friction_compensation = interp(error, [-JERK_THRESHOLD, JERK_THRESHOLD], [-self.friction, self.friction]) ff += friction_compensation / self.kf freeze_integrator = CS.steeringRateLimited or CS.steeringPressed or CS.vEgo < 5 output_torque = self.pid.update(error, diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index f2cd282198..e9458607d5 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -1,7 +1,7 @@ from cereal import car from common.numpy_fast import clip, interp from common.realtime import DT_CTRL -from selfdrive.controls.lib.drive_helpers import CONTROL_N +from selfdrive.controls.lib.drive_helpers import CONTROL_N, apply_deadzone from selfdrive.controls.lib.pid import PIDController from selfdrive.modeld.constants import T_IDXS @@ -12,16 +12,6 @@ ACCEL_MIN_ISO = -3.5 # m/s^2 ACCEL_MAX_ISO = 2.0 # m/s^2 -def apply_deadzone(error, deadzone): - if error > deadzone: - error -= deadzone - elif error < - deadzone: - error += deadzone - else: - error = 0. - return error - - def long_control_state_trans(CP, active, long_control_state, v_ego, v_target, v_target_future, brake_pressed, cruise_standstill): """Update longitudinal control state machine""" diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 298de7999a..613b7f26ae 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -935abbc21282e10572c9324382feba3fee2fe379 \ No newline at end of file +123506cad1877e93bfe5c91ecdce654ef339959b From e3750877202a072e884ad0fb88709b23226c3a59 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sat, 11 Jun 2022 00:14:58 -0700 Subject: [PATCH 40/93] Honda carcontroller and signal cleanup (#24806) * common signals * move stopping * space * clean up * bump opendbc --- opendbc | 2 +- selfdrive/car/honda/carcontroller.py | 27 ++++----------- selfdrive/car/honda/hondacan.py | 49 ++++++++++++---------------- 3 files changed, 29 insertions(+), 49 deletions(-) diff --git a/opendbc b/opendbc index ec0e1f20ba..58a2c9b2fc 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit ec0e1f20bae4c39326036c0061418404ac15ae9e +Subproject commit 58a2c9b2fc5c60ca68d3dced09f6c4c72ca72415 diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index cf01fa2aaa..a6aa84adf6 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -95,8 +95,8 @@ def process_hud_alert(hud_alert): HUDData = namedtuple("HUDData", - ["pcm_accel", "v_cruise", "car", - "lanes", "fcw", "acc_alert", "steer_required"]) + ["pcm_accel", "v_cruise", "lead_visible", + "lanes_visible", "fcw", "acc_alert", "steer_required"]) class CarController: @@ -138,19 +138,6 @@ class CarController: self.brake_last = rate_limit(pre_limit_brake, self.brake_last, -2., DT_CTRL) # vehicle hud display, wait for one update from 10Hz 0x304 msg - if hud_control.lanesVisible: - hud_lanes = 1 - else: - hud_lanes = 0 - - if CC.enabled: - if hud_control.leadVisible: - hud_car = 2 - else: - hud_car = 1 - else: - hud_car = 0 - fcw_display, steer_required, acc_alert = process_hud_alert(hud_control.visualAlert) # **** process the car messages **** @@ -172,8 +159,6 @@ class CarController: can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, CC.latActive, self.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl)) - stopping = actuators.longControlState == LongCtrlState.stopping - # wind brake from air resistance decel at high speed wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15]) # all of this is only relevant for HONDA NIDEC @@ -222,6 +207,8 @@ class CarController: if self.CP.carFingerprint in HONDA_BOSCH: self.accel = clip(accel, self.params.BOSCH_ACCEL_MIN, self.params.BOSCH_ACCEL_MAX) self.gas = interp(accel, self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V) + + stopping = actuators.longControlState == LongCtrlState.stopping can_sends.extend(hondacan.create_acc_commands(self.packer, CC.enabled, CC.longActive, self.accel, self.gas, idx, stopping, self.CP.carFingerprint)) else: @@ -252,9 +239,9 @@ class CarController: # Send dashboard UI commands. if self.frame % 10 == 0: idx = (self.frame // 10) % 4 - hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car, - hud_lanes, fcw_display, acc_alert, steer_required) - can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, pcm_speed, hud, CS.is_metric, idx, CS.stock_hud)) + hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_control.leadVisible, + hud_control.lanesVisible, fcw_display, acc_alert, steer_required) + can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, CC.enabled, pcm_speed, hud, CS.is_metric, idx, CS.stock_hud)) if self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in HONDA_BOSCH: self.speed = pcm_speed diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index dcdc0e5f94..9c9e873e18 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -98,47 +98,40 @@ def create_bosch_supplemental_1(packer, car_fingerprint, idx): return packer.make_can_msg("BOSCH_SUPPLEMENTAL_1", bus, values, idx) -def create_ui_commands(packer, CP, pcm_speed, hud, is_metric, idx, stock_hud): +def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, idx, stock_hud): commands = [] bus_pt = get_pt_bus(CP.carFingerprint) radar_disabled = CP.carFingerprint in HONDA_BOSCH and CP.openpilotLongitudinalControl bus_lkas = get_lkas_cmd_bus(CP.carFingerprint, radar_disabled) if CP.openpilotLongitudinalControl: + acc_hud_values = { + 'CRUISE_SPEED': hud.v_cruise, + 'ENABLE_MINI_CAR': 1, + 'HUD_DISTANCE': 3, # max distance setting on display + 'IMPERIAL_UNIT': int(not is_metric), + 'HUD_LEAD': 2 if enabled and hud.lead_visible else 1 if enabled else 0, + 'SET_ME_X01_2': 1, + } + if CP.carFingerprint in HONDA_BOSCH: - acc_hud_values = { - 'CRUISE_SPEED': hud.v_cruise, - 'ENABLE_MINI_CAR': 1, - 'SET_TO_1': 1, - 'HUD_LEAD': hud.car, - 'HUD_DISTANCE': 3, - 'ACC_ON': hud.car != 0, - 'SET_TO_X1': 1, - 'IMPERIAL_UNIT': int(not is_metric), - 'FCM_OFF': 1, - } + acc_hud_values['ACC_ON'] = hud.car != 0 + acc_hud_values['FCM_OFF'] = 1 + acc_hud_values['FCM_OFF_2'] = 1 else: - acc_hud_values = { - 'PCM_SPEED': pcm_speed * CV.MS_TO_KPH, - 'PCM_GAS': hud.pcm_accel, - 'CRUISE_SPEED': hud.v_cruise, - 'ENABLE_MINI_CAR': 1, - 'HUD_LEAD': hud.car, - 'HUD_DISTANCE': 3, # max distance setting on display - 'IMPERIAL_UNIT': int(not is_metric), - 'SET_ME_X01_2': 1, - 'SET_ME_X01': 1, - "FCM_OFF": stock_hud["FCM_OFF"], - "FCM_OFF_2": stock_hud["FCM_OFF_2"], - "FCM_PROBLEM": stock_hud["FCM_PROBLEM"], - "ICONS": stock_hud["ICONS"], - } + acc_hud_values['PCM_SPEED'] = pcm_speed * CV.MS_TO_KPH + acc_hud_values['PCM_GAS'] = hud.pcm_accel + acc_hud_values['SET_ME_X01'] = 1 + acc_hud_values['FCM_OFF'] = stock_hud['FCM_OFF'] + acc_hud_values['FCM_OFF_2'] = stock_hud['FCM_OFF_2'] + acc_hud_values['FCM_PROBLEM'] = stock_hud['FCM_PROBLEM'] + acc_hud_values['ICONS'] = stock_hud['ICONS'] commands.append(packer.make_can_msg("ACC_HUD", bus_pt, acc_hud_values, idx)) lkas_hud_values = { 'SET_ME_X41': 0x41, 'STEERING_REQUIRED': hud.steer_required, - 'SOLID_LANES': hud.lanes, + 'SOLID_LANES': hud.lanes_visible, 'BEEP': 0, } From 1847a70a47b12ff5c7e7db9361f69d6d1e79a919 Mon Sep 17 00:00:00 2001 From: Maykon Pacheco <38124066+maykonpacheco@users.noreply.github.com> Date: Sat, 11 Jun 2022 14:08:24 -0400 Subject: [PATCH 41/93] test for the strip_bz2_extension method (#24826) --- selfdrive/athena/tests/test_athenad.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/selfdrive/athena/tests/test_athenad.py b/selfdrive/athena/tests/test_athenad.py index b6457ca01d..a0b308c216 100755 --- a/selfdrive/athena/tests/test_athenad.py +++ b/selfdrive/athena/tests/test_athenad.py @@ -120,6 +120,13 @@ class TestAthenadMethods(unittest.TestCase): self.assertTrue(resp, 'list empty!') self.assertCountEqual(resp, expected) + def test_strip_bz2_extension(self): + fn = os.path.join(athenad.ROOT, 'qlog.bz2') + Path(fn).touch() + if fn.endswith('.bz2'): + self.assertEqual(athenad.strip_bz2_extension(fn), fn[:-4]) + + @with_http_server def test_do_upload(self, host): fn = os.path.join(athenad.ROOT, 'qlog.bz2') From a029245a784acb9fe3d8db7f1023aab222ec0dba Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sat, 11 Jun 2022 12:20:24 -0700 Subject: [PATCH 42/93] CI: fix docker push for base image --- .github/workflows/selfdrive_tests.yaml | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index 04ae841f77..567fc05aa5 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -20,8 +20,7 @@ env: RUN: docker run --shm-size 1G -v $PWD:/tmp/openpilot -w /tmp/openpilot -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v /tmp/scons_cache:/tmp/scons_cache -v /tmp/comma_download_cache:/tmp/comma_download_cache $BASE_IMAGE /bin/sh -c BUILD_CL: | - docker pull $(grep -iohP '(?<=^from)\s+\S+' Dockerfile.openpilot_base_cl) || true - docker pull $DOCKER_REGISTRY/$BASE_IMAGE:latest || true + docker pull $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest || true docker build --cache-from $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest -t $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest -t $CL_BASE_IMAGE:latest -f Dockerfile.openpilot_base_cl . RUN_CL: docker run --shm-size 1G -v $PWD:/tmp/openpilot -w /tmp/openpilot -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v /tmp/scons_cache:/tmp/scons_cache -v /tmp/comma_download_cache:/tmp/comma_download_cache $CL_BASE_IMAGE /bin/sh -c @@ -201,12 +200,15 @@ jobs: submodules: true - name: Build Docker image run: eval "$BUILD" + - name: Push to container registry + run: | + $DOCKER_LOGIN + docker push $DOCKER_REGISTRY/$BASE_IMAGE:latest - name: Build CL Docker image run: eval "$BUILD_CL" - name: Push to container registry run: | $DOCKER_LOGIN - docker push $DOCKER_REGISTRY/$BASE_IMAGE:latest docker push $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest static_analysis: From 1c238e8c773eb58f88fc69cdcb77dba71430b735 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sat, 11 Jun 2022 13:06:07 -0700 Subject: [PATCH 43/93] pylint: add PyQt5 support --- .pylintrc | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/.pylintrc b/.pylintrc index 73035360ca..27539b743c 100644 --- a/.pylintrc +++ b/.pylintrc @@ -3,7 +3,7 @@ # A comma-separated list of package or module names from where C extensions may # be loaded. Extensions are loading into the active Python interpreter and may # run arbitrary code -extension-pkg-whitelist=scipy cereal.messaging.messaging_pyx +extension-pkg-whitelist=scipy,cereal.messaging.messaging_pyx,PyQt5 # Add files or directories to the blacklist. They should be base names, not # paths. @@ -250,13 +250,6 @@ max-line-length=100 # Maximum number of lines in a module max-module-lines=1000 -# List of optional constructs for which whitespace checking is disabled. `dict- -# separator` is used to allow tabulation in dicts, etc.: {1 : 1,\n222: 2}. -# `trailing-comma` allows a space between comma and closing bracket: (a, ). -# `empty-line` allows space-only lines. -no-space-check=trailing-comma, - dict-separator - # Allow the body of a class to be on the same line as the declaration if body # contains single statement. single-line-class-stmt=no From 5add0c6159db3810c17f0b27afd3dda882f95586 Mon Sep 17 00:00:00 2001 From: Maxime Desroches Date: Sat, 11 Jun 2022 15:32:12 -0700 Subject: [PATCH 44/93] simulator: run simulator test in ci (#24691) * run simulator test in ci * block navd process * block ui * fix jenkins * build docker * remove tty * remove tty for carla * detach carla_sim * more retries * only build once * add more time for bridge * cleanup * use qt offscreen * expose to docker * block ui * use new dockerimage * fix * from ubuntu20.04 * install curl * add ssh * add locales * noninteractive * syntax * use base * smaller image * add git + git lfs * kill carla * run in parallel * fix missing agents * default agent? * little cleanup * default doesn't work * not in ci * fix path * fix path * new msg Co-authored-by: Adeeb Shihadeh --- Jenkinsfile | 129 ++++++++++-------- tools/sim/Dockerfile.sim | 2 +- tools/sim/Dockerfile.sim_nvidia | 21 +++ tools/sim/launch_openpilot.sh | 4 + tools/sim/lib/can.py | 1 + tools/sim/start_carla.sh | 7 +- tools/sim/start_openpilot_docker.sh | 22 +-- tools/sim/{test => tests}/__init__.py | 0 .../{test => tests}/test_carla_integration.py | 19 ++- 9 files changed, 133 insertions(+), 72 deletions(-) create mode 100644 tools/sim/Dockerfile.sim_nvidia rename tools/sim/{test => tests}/__init__.py (100%) rename tools/sim/{test => tests}/test_carla_integration.py (85%) diff --git a/Jenkinsfile b/Jenkinsfile index 3b134d7c0d..fed587bd21 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -42,6 +42,7 @@ def phone_steps(String device_type, steps) { pipeline { agent none environment { + CI = "1" TEST_DIR = "/data/openpilot" SOURCE_DIR = "/data/openpilot_source/" } @@ -74,71 +75,89 @@ pipeline { } } - stages { - stage('On-device Tests') { - agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } - stages { - stage('parallel tests') { - parallel { - stage('build') { - environment { - R3_PUSH = "${env.BRANCH_NAME == 'master' ? '1' : ' '}" - } - steps { - phone_steps("tici", [ - ["build master-ci", "cd $SOURCE_DIR/release && TARGET_DIR=$TEST_DIR EXTRA_FILES='tools/' ./build_devel.sh"], - ["build openpilot", "cd selfdrive/manager && ./build.py"], - ["test manager", "python selfdrive/manager/test/test_manager.py"], - ["onroad tests", "cd selfdrive/test/ && ./test_onroad.py"], - ["test car interfaces", "cd selfdrive/car/tests/ && ./test_car_interfaces.py"], - ]) - } - } - - stage('HW + Unit Tests') { - steps { - phone_steps("tici2", [ - ["build", "cd selfdrive/manager && ./build.py"], - ["test power draw", "python selfdrive/hardware/tici/test_power_draw.py"], - ["test boardd loopback", "python selfdrive/boardd/tests/test_boardd_loopback.py"], - ["test loggerd", "python selfdrive/loggerd/tests/test_loggerd.py"], - ["test encoder", "LD_LIBRARY_PATH=/usr/local/lib python selfdrive/loggerd/tests/test_encoder.py"], - ["test sensord", "python selfdrive/sensord/test/test_sensord.py"], - ]) - } - } - - stage('camerad') { - steps { - phone_steps("tici-party", [ - ["build", "cd selfdrive/manager && ./build.py"], - ["test camerad", "python selfdrive/camerad/test/test_camerad.py"], - ["test exposure", "python selfdrive/camerad/test/test_exposure.py"], - ]) - } - } - - stage('replay') { - steps { - phone_steps("tici3", [ - ["build", "cd selfdrive/manager && ./build.py"], - ["model replay", "cd selfdrive/test/process_replay && ./model_replay.py"], - ]) - } - } - - } + parallel { + + stage('simulator') { + agent { + dockerfile { + filename 'Dockerfile.sim_nvidia' + dir 'tools/sim' + args '--user=root' } } + steps { + sh "git config --global --add safe.directory ${WORKSPACE}" + sh "git lfs pull" + sh "${WORKSPACE}/tools/sim/build_container.sh" + sh "DETACH=1 ${WORKSPACE}/tools/sim/start_carla.sh" + sh "${WORKSPACE}/tools/sim/start_openpilot_docker.sh" + } post { always { - cleanWs() + sh "docker kill carla_sim || true" + sh "rm -rf ${WORKSPACE}/* || true" + sh "rm -rf .* || true" } } + } + stage('build') { + agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } + environment { + R3_PUSH = "${env.BRANCH_NAME == 'master' ? '1' : ' '}" + } + steps { + phone_steps("tici", [ + ["build master-ci", "cd $SOURCE_DIR/release && TARGET_DIR=$TEST_DIR EXTRA_FILES='tools/' ./build_devel.sh"], + ["build openpilot", "cd selfdrive/manager && ./build.py"], + ["test manager", "python selfdrive/manager/test/test_manager.py"], + ["onroad tests", "cd selfdrive/test/ && ./test_onroad.py"], + ["test car interfaces", "cd selfdrive/car/tests/ && ./test_car_interfaces.py"], + ]) + } + } + + stage('HW + Unit Tests') { + agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } + steps { + phone_steps("tici2", [ + ["build", "cd selfdrive/manager && ./build.py"], + ["test power draw", "python selfdrive/hardware/tici/test_power_draw.py"], + ["test boardd loopback", "python selfdrive/boardd/tests/test_boardd_loopback.py"], + ["test loggerd", "python selfdrive/loggerd/tests/test_loggerd.py"], + ["test encoder", "LD_LIBRARY_PATH=/usr/local/lib python selfdrive/loggerd/tests/test_encoder.py"], + ["test sensord", "python selfdrive/sensord/test/test_sensord.py"], + ]) + } } + stage('camerad') { + agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } + steps { + phone_steps("tici-party", [ + ["build", "cd selfdrive/manager && ./build.py"], + ["test camerad", "python selfdrive/camerad/test/test_camerad.py"], + ["test exposure", "python selfdrive/camerad/test/test_exposure.py"], + ]) + } + } + + stage('replay') { + agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } + steps { + phone_steps("tici3", [ + ["build", "cd selfdrive/manager && ./build.py"], + ["model replay", "cd selfdrive/test/process_replay && ./model_replay.py"], + ]) + } + } + } + + post { + always { + cleanWs() + } } } } diff --git a/tools/sim/Dockerfile.sim b/tools/sim/Dockerfile.sim index d869db90a6..d838efed13 100644 --- a/tools/sim/Dockerfile.sim +++ b/tools/sim/Dockerfile.sim @@ -27,6 +27,6 @@ COPY ./system $HOME/openpilot/system COPY ./tools $HOME/openpilot/tools WORKDIR $HOME/openpilot -RUN scons -j$(nproc) +RUN scons -j12 RUN python -c "from selfdrive.test.helpers import set_params_enabled; set_params_enabled()" diff --git a/tools/sim/Dockerfile.sim_nvidia b/tools/sim/Dockerfile.sim_nvidia new file mode 100644 index 0000000000..5e5dd263da --- /dev/null +++ b/tools/sim/Dockerfile.sim_nvidia @@ -0,0 +1,21 @@ +FROM ubuntu:20.04 + +ENV DEBIAN_FRONTEND=noninteractive +RUN apt-get update && \ + apt-get install -y --no-install-recommends \ + apt-utils \ + sudo \ + ssh \ + curl \ + ca-certificates \ + git \ + git-lfs && \ + rm -rf /var/lib/apt/lists/* + +RUN curl -fsSL https://get.docker.com -o get-docker.sh && \ + sudo sh get-docker.sh && \ + distribution=$(. /etc/os-release;echo $ID$VERSION_ID) && \ + curl -s -L https://nvidia.github.io/nvidia-docker/gpgkey | sudo apt-key add - && \ + curl -s -L https://nvidia.github.io/nvidia-docker/$distribution/nvidia-docker.list | sudo tee /etc/apt/sources.list.d/nvidia-docker.list && \ + sudo apt-get update && \ + sudo apt-get install -y nvidia-docker2 diff --git a/tools/sim/launch_openpilot.sh b/tools/sim/launch_openpilot.sh index 82fc4a71ac..15f45b4cc2 100755 --- a/tools/sim/launch_openpilot.sh +++ b/tools/sim/launch_openpilot.sh @@ -6,6 +6,10 @@ export SIMULATION="1" export FINGERPRINT="HONDA CIVIC 2016" export BLOCK="camerad,loggerd,encoderd" +if [[ "$CI" ]]; then + # TODO: offscreen UI should work + export BLOCK="${BLOCK},ui" +fi DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" cd ../../selfdrive/manager && exec ./manager.py diff --git a/tools/sim/lib/can.py b/tools/sim/lib/can.py index af0f415bbc..2b1048fef2 100755 --- a/tools/sim/lib/can.py +++ b/tools/sim/lib/can.py @@ -63,6 +63,7 @@ def can_function(pm, speed, angle, idx, cruise_button, is_engaged): msg.append(packer.make_can_msg("SCM_FEEDBACK", 0, {"MAIN_ON": 1}, idx)) msg.append(packer.make_can_msg("POWERTRAIN_DATA", 0, {"ACC_STATUS": int(is_engaged)}, idx)) msg.append(packer.make_can_msg("HUD_SETTING", 0, {})) + msg.append(packer.make_can_msg("CAR_SPEED", 0, {})) # *** cam bus *** msg.append(packer.make_can_msg("STEERING_CONTROL", 2, {}, idx)) diff --git a/tools/sim/start_carla.sh b/tools/sim/start_carla.sh index 912c7d6f08..abdaee4ea8 100755 --- a/tools/sim/start_carla.sh +++ b/tools/sim/start_carla.sh @@ -17,12 +17,17 @@ fi docker pull carlasim/carla:0.9.12 +EXTRA_ARGS="-it" +if [[ "$DETACH" ]]; then + EXTRA_ARGS="-d" +fi + docker run \ --name carla_sim \ --rm \ --gpus all \ --net=host \ -v /tmp/.X11-unix:/tmp/.X11-unix:rw \ - -it \ + $EXTRA_ARGS \ carlasim/carla:0.9.12 \ /bin/bash ./CarlaUE4.sh -opengl -nosound -RenderOffScreen -benchmark -fps=20 -quality-level=Low diff --git a/tools/sim/start_openpilot_docker.sh b/tools/sim/start_openpilot_docker.sh index 106dbdeccb..e48e63574d 100755 --- a/tools/sim/start_openpilot_docker.sh +++ b/tools/sim/start_openpilot_docker.sh @@ -3,22 +3,26 @@ DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" cd $DIR -# expose X to the container -xhost +local:root - -docker pull ghcr.io/commaai/openpilot-sim:latest - OPENPILOT_DIR="/openpilot" -if ! [[ -z "$MOUNT_OPENPILOT" ]] -then +if ! [[ -z "$MOUNT_OPENPILOT" ]]; then OPENPILOT_DIR="$(dirname $(dirname $DIR))" EXTRA_ARGS="-v $OPENPILOT_DIR:$OPENPILOT_DIR -e PYTHONPATH=$OPENPILOT_DIR:$PYTHONPATH" fi +if [[ "$CI" ]]; then + CMD="CI=1 ${OPENPILOT_DIR}/tools/sim/tests/test_carla_integration.py" +else + # expose X to the container + xhost +local:root + + docker pull ghcr.io/commaai/openpilot-sim:latest + CMD="./tmux_script.sh $*" + EXTRA_ARGS="${EXTRA_ARGS} -it" +fi + docker run --net=host\ --name openpilot_client \ --rm \ - -it \ --gpus all \ --device=/dev/dri:/dev/dri \ --device=/dev/input:/dev/input \ @@ -29,4 +33,4 @@ docker run --net=host\ -w "$OPENPILOT_DIR/tools/sim" \ $EXTRA_ARGS \ ghcr.io/commaai/openpilot-sim:latest \ - /bin/bash -c "./tmux_script.sh $*" + /bin/bash -c "$CMD" diff --git a/tools/sim/test/__init__.py b/tools/sim/tests/__init__.py similarity index 100% rename from tools/sim/test/__init__.py rename to tools/sim/tests/__init__.py diff --git a/tools/sim/test/test_carla_integration.py b/tools/sim/tests/test_carla_integration.py similarity index 85% rename from tools/sim/test/test_carla_integration.py rename to tools/sim/tests/test_carla_integration.py index 8f76abb561..43db783f4e 100755 --- a/tools/sim/test/test_carla_integration.py +++ b/tools/sim/tests/test_carla_integration.py @@ -2,13 +2,18 @@ import subprocess import time import unittest +import os from multiprocessing import Queue from cereal import messaging +from common.basedir import BASEDIR from selfdrive.manager.helpers import unblock_stdout from tools.sim import bridge from tools.sim.bridge import CarlaBridge +CI = "CI" in os.environ + +SIM_DIR = os.path.join(BASEDIR, "tools/sim") class TestCarlaIntegration(unittest.TestCase): """ @@ -19,10 +24,12 @@ class TestCarlaIntegration(unittest.TestCase): def setUp(self): self.processes = [] - # We want to make sure that carla_sim docker isn't still running. - subprocess.run("docker rm -f carla_sim", shell=True, stderr=subprocess.PIPE, check=False) - self.carla_process = subprocess.Popen(".././start_carla.sh") + if not CI: + # We want to make sure that carla_sim docker isn't still running. + subprocess.run("docker rm -f carla_sim", shell=True, stderr=subprocess.PIPE, check=False) + self.carla_process = subprocess.Popen("./start_carla.sh", cwd=SIM_DIR) + # Too many lagging messages in bridge.py can cause a crash. This prevents it. unblock_stdout() # Wait 10 seconds to startup carla @@ -30,16 +37,16 @@ class TestCarlaIntegration(unittest.TestCase): def test_engage(self): # Startup manager and bridge.py. Check processes are running, then engage and verify. - p_manager = subprocess.Popen("./launch_openpilot.sh", cwd='../') + p_manager = subprocess.Popen("./launch_openpilot.sh", cwd=SIM_DIR) self.processes.append(p_manager) sm = messaging.SubMaster(['controlsState', 'carEvents', 'managerState']) q = Queue() carla_bridge = CarlaBridge(bridge.parse_args([])) - p_bridge = carla_bridge.run(q, retries=3) + p_bridge = carla_bridge.run(q, retries=10) self.processes.append(p_bridge) - max_time_per_step = 20 + max_time_per_step = 60 # Wait for bridge to startup start_waiting = time.monotonic() From 1139fe507b01f34de9714c99228f411558b44231 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sat, 11 Jun 2022 16:38:24 -0700 Subject: [PATCH 45/93] Move selfdrive/hardware/ to system/ (#24725) * move hardware to system/ * fix mypy --- Jenkinsfile | 2 +- common/basedir.py | 2 +- common/modeldata.h | 2 +- common/params.cc | 2 +- common/realtime.py | 2 +- common/swaglog.cc | 2 +- common/tests/test_swaglog.cc | 2 +- launch_chffrplus.sh | 6 ++-- release/files_common | 30 +++++++++--------- scripts/disable-powersave.py | 2 +- selfdrive/athena/athenad.py | 2 +- selfdrive/athena/registration.py | 2 +- selfdrive/boardd/boardd.cc | 2 +- selfdrive/boardd/main.cc | 2 +- selfdrive/boardd/pandad.py | 2 +- .../boardd/tests/test_boardd_loopback.py | 2 +- selfdrive/camerad/cameras/camera_common.cc | 2 +- selfdrive/camerad/cameras/camera_common.h | 2 +- selfdrive/camerad/main.cc | 2 +- selfdrive/camerad/snapshot/snapshot.py | 2 +- selfdrive/camerad/test/test_camerad.py | 2 +- selfdrive/camerad/test/test_exposure.py | 2 +- selfdrive/controls/controlsd.py | 2 +- selfdrive/hardware | 1 + selfdrive/loggerd/config.py | 2 +- selfdrive/loggerd/logger.h | 2 +- selfdrive/loggerd/loggerd.h | 2 +- selfdrive/loggerd/tests/test_encoder.py | 2 +- selfdrive/loggerd/uploader.py | 2 +- selfdrive/manager/build.py | 2 +- selfdrive/manager/manager.py | 2 +- selfdrive/manager/process.py | 2 +- selfdrive/manager/process_config.py | 2 +- selfdrive/manager/test/test_manager.py | 2 +- selfdrive/modeld/modeld.cc | 2 +- selfdrive/modeld/models/dmonitoring.cc | 2 +- selfdrive/modeld/thneed/compile.cc | 2 +- selfdrive/sensord/test/test_sensord.py | 2 +- selfdrive/sentry.py | 2 +- selfdrive/statsd.py | 2 +- selfdrive/swaglog.py | 2 +- selfdrive/test/helpers.py | 2 +- selfdrive/test/process_replay/model_replay.py | 2 +- selfdrive/test/process_replay/test_debayer.py | 2 +- selfdrive/thermald/power_monitoring.py | 2 +- selfdrive/thermald/thermald.py | 2 +- selfdrive/timezoned.py | 2 +- selfdrive/ui/main.cc | 2 +- selfdrive/ui/qt/api.cc | 2 +- selfdrive/ui/qt/maps/map_helpers.cc | 2 +- selfdrive/ui/qt/offroad/settings.cc | 2 +- selfdrive/ui/qt/qt_window.h | 2 +- selfdrive/ui/qt/setup/setup.cc | 2 +- selfdrive/ui/qt/setup/updater.cc | 2 +- selfdrive/ui/qt/spinner.cc | 2 +- selfdrive/ui/qt/text.cc | 2 +- selfdrive/ui/qt/util.cc | 2 +- selfdrive/ui/qt/widgets/input.cc | 2 +- selfdrive/ui/qt/widgets/offroad_alerts.cc | 2 +- selfdrive/ui/qt/widgets/ssh_keys.h | 2 +- selfdrive/ui/qt/window.cc | 2 +- selfdrive/ui/replay/replay.cc | 2 +- selfdrive/ui/replay/route.cc | 2 +- selfdrive/ui/soundd/sound.h | 2 +- selfdrive/ui/tests/test_soundd.py | 2 +- selfdrive/ui/ui.cc | 2 +- selfdrive/updated.py | 6 ++-- {selfdrive/hardware/pc => system}/__init__.py | 0 {selfdrive => system}/hardware/.gitignore | 0 {selfdrive => system}/hardware/__init__.py | 6 ++-- {selfdrive => system}/hardware/base.h | 0 {selfdrive => system}/hardware/base.py | 0 {selfdrive => system}/hardware/hw.h | 4 +-- .../tici => system/hardware/pc}/__init__.py | 0 {selfdrive => system}/hardware/pc/hardware.py | 2 +- system/hardware/tici/__init__.py | 0 .../hardware/tici/agnos.json | 0 {selfdrive => system}/hardware/tici/agnos.py | 0 .../hardware/tici/amplifier.py | 0 .../hardware/tici/hardware.h | 2 +- .../hardware/tici/hardware.py | 8 ++--- {selfdrive => system}/hardware/tici/iwlist.py | 0 {selfdrive => system}/hardware/tici/pins.py | 0 .../hardware/tici/power_draw_test.py | 4 +-- .../hardware/tici/power_monitor.py | 0 .../hardware/tici/precise_power_measure.py | 2 +- .../hardware/tici/restart_modem.sh | 0 .../hardware/tici/test_agnos_updater.py | 0 .../hardware/tici/test_power_draw.py | 4 +-- {selfdrive => system}/hardware/tici/updater | Bin tools/lib/auth_config.py | 2 +- 91 files changed, 102 insertions(+), 101 deletions(-) create mode 120000 selfdrive/hardware rename {selfdrive/hardware/pc => system}/__init__.py (100%) rename {selfdrive => system}/hardware/.gitignore (100%) rename {selfdrive => system}/hardware/__init__.py (56%) rename {selfdrive => system}/hardware/base.h (100%) rename {selfdrive => system}/hardware/base.py (100%) rename {selfdrive => system}/hardware/hw.h (92%) rename {selfdrive/hardware/tici => system/hardware/pc}/__init__.py (100%) rename {selfdrive => system}/hardware/pc/hardware.py (96%) create mode 100644 system/hardware/tici/__init__.py rename {selfdrive => system}/hardware/tici/agnos.json (100%) rename {selfdrive => system}/hardware/tici/agnos.py (100%) rename {selfdrive => system}/hardware/tici/amplifier.py (100%) rename {selfdrive => system}/hardware/tici/hardware.h (97%) rename {selfdrive => system}/hardware/tici/hardware.py (98%) rename {selfdrive => system}/hardware/tici/iwlist.py (100%) rename {selfdrive => system}/hardware/tici/pins.py (100%) rename {selfdrive => system}/hardware/tici/power_draw_test.py (97%) rename {selfdrive => system}/hardware/tici/power_monitor.py (100%) rename {selfdrive => system}/hardware/tici/precise_power_measure.py (77%) rename {selfdrive => system}/hardware/tici/restart_modem.sh (100%) rename {selfdrive => system}/hardware/tici/test_agnos_updater.py (100%) rename {selfdrive => system}/hardware/tici/test_power_draw.py (92%) rename {selfdrive => system}/hardware/tici/updater (100%) diff --git a/Jenkinsfile b/Jenkinsfile index fed587bd21..250893d1af 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -123,7 +123,7 @@ pipeline { steps { phone_steps("tici2", [ ["build", "cd selfdrive/manager && ./build.py"], - ["test power draw", "python selfdrive/hardware/tici/test_power_draw.py"], + ["test power draw", "python system/hardware/tici/test_power_draw.py"], ["test boardd loopback", "python selfdrive/boardd/tests/test_boardd_loopback.py"], ["test loggerd", "python selfdrive/loggerd/tests/test_loggerd.py"], ["test encoder", "LD_LIBRARY_PATH=/usr/local/lib python selfdrive/loggerd/tests/test_encoder.py"], diff --git a/common/basedir.py b/common/basedir.py index 8be1cf6afa..371b54d3ef 100644 --- a/common/basedir.py +++ b/common/basedir.py @@ -1,7 +1,7 @@ import os from pathlib import Path -from selfdrive.hardware import PC +from system.hardware import PC BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")) diff --git a/common/modeldata.h b/common/modeldata.h index 06d9398031..aee9fdfd83 100644 --- a/common/modeldata.h +++ b/common/modeldata.h @@ -2,7 +2,7 @@ #include #include "common/mat.h" -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" const int TRAJECTORY_SIZE = 33; const int LAT_MPC_N = 16; diff --git a/common/params.cc b/common/params.cc index 330eddc1a9..b740e5a71e 100644 --- a/common/params.cc +++ b/common/params.cc @@ -8,7 +8,7 @@ #include "common/swaglog.h" #include "common/util.h" -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" namespace { diff --git a/common/realtime.py b/common/realtime.py index 03051c6f67..8a79d8d39f 100644 --- a/common/realtime.py +++ b/common/realtime.py @@ -8,7 +8,7 @@ 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 +from system.hardware import PC # time step for each process diff --git a/common/swaglog.cc b/common/swaglog.cc index 75223854fe..6b0028326a 100644 --- a/common/swaglog.cc +++ b/common/swaglog.cc @@ -15,7 +15,7 @@ #include "common/util.h" #include "common/version.h" -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" class SwaglogState : public LogState { public: diff --git a/common/tests/test_swaglog.cc b/common/tests/test_swaglog.cc index b54d1d6338..20455ec74c 100644 --- a/common/tests/test_swaglog.cc +++ b/common/tests/test_swaglog.cc @@ -7,7 +7,7 @@ #include "common/swaglog.h" #include "common/util.h" #include "common/version.h" -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" const char *SWAGLOG_ADDR = "ipc:///tmp/logmessage"; std::string daemon_name = "testy"; diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index a37e27c4fe..911774a4eb 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -22,12 +22,12 @@ function agnos_init { # Check if AGNOS update is required if [ $(< /VERSION) != "$AGNOS_VERSION" ]; then - AGNOS_PY="$DIR/selfdrive/hardware/tici/agnos.py" - MANIFEST="$DIR/selfdrive/hardware/tici/agnos.json" + AGNOS_PY="$DIR/system/hardware/tici/agnos.py" + MANIFEST="$DIR/system/hardware/tici/agnos.json" if $AGNOS_PY --verify $MANIFEST; then sudo reboot fi - $DIR/selfdrive/hardware/tici/updater $AGNOS_PY $MANIFEST + $DIR/system/hardware/tici/updater $AGNOS_PY $MANIFEST fi } diff --git a/release/files_common b/release/files_common index 5230511ddd..e89163aba0 100644 --- a/release/files_common +++ b/release/files_common @@ -182,21 +182,21 @@ selfdrive/controls/lib/longitudinal_mpc_lib/.gitignore selfdrive/controls/lib/lateral_mpc_lib/* selfdrive/controls/lib/longitudinal_mpc_lib/* -selfdrive/hardware/__init__.py -selfdrive/hardware/base.h -selfdrive/hardware/base.py -selfdrive/hardware/hw.h -selfdrive/hardware/tici/__init__.py -selfdrive/hardware/tici/hardware.h -selfdrive/hardware/tici/hardware.py -selfdrive/hardware/tici/pins.py -selfdrive/hardware/tici/agnos.py -selfdrive/hardware/tici/agnos.json -selfdrive/hardware/tici/amplifier.py -selfdrive/hardware/tici/updater -selfdrive/hardware/tici/iwlist.py -selfdrive/hardware/pc/__init__.py -selfdrive/hardware/pc/hardware.py +system/hardware/__init__.py +system/hardware/base.h +system/hardware/base.py +system/hardware/hw.h +system/hardware/tici/__init__.py +system/hardware/tici/hardware.h +system/hardware/tici/hardware.py +system/hardware/tici/pins.py +system/hardware/tici/agnos.py +system/hardware/tici/agnos.json +system/hardware/tici/amplifier.py +system/hardware/tici/updater +system/hardware/tici/iwlist.py +system/hardware/pc/__init__.py +system/hardware/pc/hardware.py selfdrive/locationd/__init__.py selfdrive/locationd/.gitignore diff --git a/scripts/disable-powersave.py b/scripts/disable-powersave.py index f651bc87f1..93688504f3 100755 --- a/scripts/disable-powersave.py +++ b/scripts/disable-powersave.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -from selfdrive.hardware import HARDWARE +from system.hardware import HARDWARE if __name__ == "__main__": HARDWARE.set_power_save(False) diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index ba86e02cc6..0dc7d704a8 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, AGNOS +from system.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 diff --git a/selfdrive/athena/registration.py b/selfdrive/athena/registration.py index 2748934361..c44ca4c28b 100755 --- a/selfdrive/athena/registration.py +++ b/selfdrive/athena/registration.py @@ -11,7 +11,7 @@ from common.params import Params from common.spinner import Spinner from common.basedir import PERSIST from selfdrive.controls.lib.alertmanager import set_offroad_alert -from selfdrive.hardware import HARDWARE, PC +from system.hardware import HARDWARE, PC from selfdrive.swaglog import cloudlog diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 2745c037b6..f47b5936be 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -27,7 +27,7 @@ #include "common/swaglog.h" #include "common/timing.h" #include "common/util.h" -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" #include "selfdrive/boardd/pigeon.h" diff --git a/selfdrive/boardd/main.cc b/selfdrive/boardd/main.cc index 6f1f83685b..cb17a584bd 100644 --- a/selfdrive/boardd/main.cc +++ b/selfdrive/boardd/main.cc @@ -3,7 +3,7 @@ #include "selfdrive/boardd/boardd.h" #include "common/swaglog.h" #include "common/util.h" -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" int main(int argc, char *argv[]) { LOGW("starting boardd"); diff --git a/selfdrive/boardd/pandad.py b/selfdrive/boardd/pandad.py index 306d764e59..51bfb92dd9 100755 --- a/selfdrive/boardd/pandad.py +++ b/selfdrive/boardd/pandad.py @@ -10,7 +10,7 @@ from functools import cmp_to_key from panda import DEFAULT_FW_FN, DEFAULT_H7_FW_FN, MCU_TYPE_H7, Panda, PandaDFU from common.basedir import BASEDIR from common.params import Params -from selfdrive.hardware import HARDWARE +from system.hardware import HARDWARE from selfdrive.swaglog import cloudlog diff --git a/selfdrive/boardd/tests/test_boardd_loopback.py b/selfdrive/boardd/tests/test_boardd_loopback.py index 631b4c987f..e9bbcb4586 100755 --- a/selfdrive/boardd/tests/test_boardd_loopback.py +++ b/selfdrive/boardd/tests/test_boardd_loopback.py @@ -12,7 +12,7 @@ from common.spinner import Spinner from common.timeout import Timeout from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.car import make_can_msg -from selfdrive.hardware import TICI +from system.hardware import TICI from selfdrive.test.helpers import phone_only, with_processes diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc index 049324f085..a94cfedf1a 100644 --- a/selfdrive/camerad/cameras/camera_common.cc +++ b/selfdrive/camerad/cameras/camera_common.cc @@ -15,7 +15,7 @@ #include "common/modeldata.h" #include "common/swaglog.h" #include "common/util.h" -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" #include "msm_media_info.h" #ifdef QCOM2 diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h index 74d6a6eb3f..e9c7ccd757 100644 --- a/selfdrive/camerad/cameras/camera_common.h +++ b/selfdrive/camerad/cameras/camera_common.h @@ -13,7 +13,7 @@ #include "common/mat.h" #include "common/queue.h" #include "common/swaglog.h" -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" #define CAMERA_ID_IMX298 0 #define CAMERA_ID_IMX179 1 diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc index c86543265c..32899a8547 100644 --- a/selfdrive/camerad/main.cc +++ b/selfdrive/camerad/main.cc @@ -4,7 +4,7 @@ #include "common/params.h" #include "common/util.h" -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" int main(int argc, char *argv[]) { if (!Hardware::PC()) { diff --git a/selfdrive/camerad/snapshot/snapshot.py b/selfdrive/camerad/snapshot/snapshot.py index 9f9072c962..fa88849b69 100755 --- a/selfdrive/camerad/snapshot/snapshot.py +++ b/selfdrive/camerad/snapshot/snapshot.py @@ -9,7 +9,7 @@ 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 PC +from system.hardware import PC from selfdrive.controls.lib.alertmanager import set_offroad_alert from selfdrive.manager.process_config import managed_processes diff --git a/selfdrive/camerad/test/test_camerad.py b/selfdrive/camerad/test/test_camerad.py index c311c17169..1a2e365a8f 100755 --- a/selfdrive/camerad/test/test_camerad.py +++ b/selfdrive/camerad/test/test_camerad.py @@ -4,7 +4,7 @@ import time import unittest import cereal.messaging as messaging -from selfdrive.hardware import TICI +from system.hardware import TICI from selfdrive.test.helpers import with_processes TEST_TIMESPAN = 30 # random.randint(60, 180) # seconds diff --git a/selfdrive/camerad/test/test_exposure.py b/selfdrive/camerad/test/test_exposure.py index 31bcc28681..f42d0bfbe3 100755 --- a/selfdrive/camerad/test/test_exposure.py +++ b/selfdrive/camerad/test/test_exposure.py @@ -6,7 +6,7 @@ import numpy as np from selfdrive.test.helpers import with_processes from selfdrive.camerad.snapshot.snapshot import get_snapshots -from selfdrive.hardware import TICI +from system.hardware import TICI TEST_TIME = 45 REPEAT = 5 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index edb1538c31..31807b0187 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 +from system.hardware import HARDWARE from selfdrive.manager.process_config import managed_processes SOFT_DISABLE_TIME = 3 # seconds diff --git a/selfdrive/hardware b/selfdrive/hardware new file mode 120000 index 0000000000..02a42c502f --- /dev/null +++ b/selfdrive/hardware @@ -0,0 +1 @@ +../system/hardware/ \ No newline at end of file diff --git a/selfdrive/loggerd/config.py b/selfdrive/loggerd/config.py index 6cd20a68ab..168c9fba91 100644 --- a/selfdrive/loggerd/config.py +++ b/selfdrive/loggerd/config.py @@ -1,6 +1,6 @@ import os from pathlib import Path -from selfdrive.hardware import PC +from system.hardware import PC if os.environ.get('LOG_ROOT', False): ROOT = os.environ['LOG_ROOT'] diff --git a/selfdrive/loggerd/logger.h b/selfdrive/loggerd/logger.h index ca08e64717..e7594cee88 100644 --- a/selfdrive/loggerd/logger.h +++ b/selfdrive/loggerd/logger.h @@ -13,7 +13,7 @@ #include "cereal/messaging/messaging.h" #include "common/util.h" #include "common/swaglog.h" -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" const std::string LOG_ROOT = Path::log_root(); diff --git a/selfdrive/loggerd/loggerd.h b/selfdrive/loggerd/loggerd.h index 3b9b01a0fc..7e13e90e63 100644 --- a/selfdrive/loggerd/loggerd.h +++ b/selfdrive/loggerd/loggerd.h @@ -20,7 +20,7 @@ #include "common/swaglog.h" #include "common/timing.h" #include "common/util.h" -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" #include "selfdrive/loggerd/encoder/encoder.h" #include "selfdrive/loggerd/logger.h" diff --git a/selfdrive/loggerd/tests/test_encoder.py b/selfdrive/loggerd/tests/test_encoder.py index 49baa8a6e3..1b9bcef2d7 100755 --- a/selfdrive/loggerd/tests/test_encoder.py +++ b/selfdrive/loggerd/tests/test_encoder.py @@ -13,7 +13,7 @@ from tqdm import trange from common.params import Params from common.timeout import Timeout -from selfdrive.hardware import TICI +from system.hardware import TICI from selfdrive.loggerd.config import ROOT from selfdrive.manager.process_config import managed_processes from tools.lib.logreader import LogReader diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index b7d8df861a..9de1c421bd 100644 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -15,7 +15,7 @@ import cereal.messaging as messaging from common.api import Api from common.params import Params from common.realtime import set_core_affinity -from selfdrive.hardware import TICI +from system.hardware import TICI from selfdrive.loggerd.xattr_cache import getxattr, setxattr from selfdrive.loggerd.config import ROOT from selfdrive.swaglog import cloudlog diff --git a/selfdrive/manager/build.py b/selfdrive/manager/build.py index 8421605a55..f0a5aec927 100755 --- a/selfdrive/manager/build.py +++ b/selfdrive/manager/build.py @@ -10,7 +10,7 @@ from pathlib import Path from common.basedir import BASEDIR from common.spinner import Spinner from common.text_window import TextWindow -from selfdrive.hardware import AGNOS +from system.hardware import AGNOS from selfdrive.swaglog import cloudlog, add_file_handler from selfdrive.version import is_dirty diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index cd7817fa97..94af397c7d 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -13,7 +13,7 @@ from common.basedir import BASEDIR from common.params import Params, ParamKeyType from common.text_window import TextWindow from selfdrive.boardd.set_time import set_time -from selfdrive.hardware import HARDWARE, PC +from system.hardware import HARDWARE, PC from selfdrive.manager.helpers import unblock_stdout from selfdrive.manager.process import ensure_running from selfdrive.manager.process_config import managed_processes diff --git a/selfdrive/manager/process.py b/selfdrive/manager/process.py index e2bb41c217..22f55f3b36 100644 --- a/selfdrive/manager/process.py +++ b/selfdrive/manager/process.py @@ -17,7 +17,7 @@ from common.basedir import BASEDIR from common.params import Params from common.realtime import sec_since_boot from selfdrive.swaglog import cloudlog -from selfdrive.hardware import HARDWARE +from system.hardware import HARDWARE from cereal import log WATCHDOG_FN = "/dev/shm/wd_" diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 5d996d1169..443ebbb419 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 PC +from system.hardware import PC from selfdrive.manager.process import PythonProcess, NativeProcess, DaemonProcess WEBCAM = os.getenv("USE_WEBCAM") is not None diff --git a/selfdrive/manager/test/test_manager.py b/selfdrive/manager/test/test_manager.py index 31949de0b9..e4c3ef7c4c 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 AGNOS, HARDWARE +from system.hardware import AGNOS, HARDWARE from selfdrive.manager.process import DaemonProcess from selfdrive.manager.process_config import managed_processes diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index 10cc2fe56e..0aac9b3c4a 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -11,7 +11,7 @@ #include "common/params.h" #include "common/swaglog.h" #include "common/util.h" -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" #include "selfdrive/modeld/models/driving.h" ExitHandler do_exit; diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc index e134ad3a5a..71da8dad53 100644 --- a/selfdrive/modeld/models/dmonitoring.cc +++ b/selfdrive/modeld/models/dmonitoring.cc @@ -6,7 +6,7 @@ #include "common/modeldata.h" #include "common/params.h" #include "common/timing.h" -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" #include "selfdrive/modeld/models/dmonitoring.h" diff --git a/selfdrive/modeld/thneed/compile.cc b/selfdrive/modeld/thneed/compile.cc index 8698ce482e..f76c63b2b9 100644 --- a/selfdrive/modeld/thneed/compile.cc +++ b/selfdrive/modeld/thneed/compile.cc @@ -3,7 +3,7 @@ #include "selfdrive/modeld/runners/snpemodel.h" #include "selfdrive/modeld/thneed/thneed.h" -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" #define TEMPORAL_SIZE 512 #define DESIRE_LEN 8 diff --git a/selfdrive/sensord/test/test_sensord.py b/selfdrive/sensord/test/test_sensord.py index 96d8891265..9fd918c971 100755 --- a/selfdrive/sensord/test/test_sensord.py +++ b/selfdrive/sensord/test/test_sensord.py @@ -4,7 +4,7 @@ import time import unittest import cereal.messaging as messaging -from selfdrive.hardware import TICI +from system.hardware import TICI from selfdrive.test.helpers import with_processes TEST_TIMESPAN = 10 diff --git a/selfdrive/sentry.py b/selfdrive/sentry.py index 5f22bf18e0..10e43ee03c 100644 --- a/selfdrive/sentry.py +++ b/selfdrive/sentry.py @@ -5,7 +5,7 @@ from sentry_sdk.integrations.threading import ThreadingIntegration from common.params import Params from selfdrive.athena.registration import is_registered_device -from selfdrive.hardware import HARDWARE, PC +from system.hardware import HARDWARE, PC from selfdrive.swaglog import cloudlog from selfdrive.version import get_branch, get_commit, get_origin, get_version, \ is_comma_remote, is_dirty, is_tested_branch diff --git a/selfdrive/statsd.py b/selfdrive/statsd.py index 5755e5111b..a48ba56003 100755 --- a/selfdrive/statsd.py +++ b/selfdrive/statsd.py @@ -10,7 +10,7 @@ from typing import NoReturn, Union, List, Dict from common.params import Params from cereal.messaging import SubMaster from selfdrive.swaglog import cloudlog -from selfdrive.hardware import HARDWARE +from system.hardware import HARDWARE from common.file_helpers import atomic_write_in_dir from selfdrive.version import get_normalized_origin, get_short_branch, get_short_version, is_dirty from selfdrive.loggerd.config import STATS_DIR, STATS_DIR_FILE_LIMIT, STATS_SOCKET, STATS_FLUSH_TIME_S diff --git a/selfdrive/swaglog.py b/selfdrive/swaglog.py index a987bfe72c..68664330a5 100644 --- a/selfdrive/swaglog.py +++ b/selfdrive/swaglog.py @@ -7,7 +7,7 @@ from logging.handlers import BaseRotatingHandler import zmq from common.logging_extra import SwagLogger, SwagFormatter, SwagLogFileFormatter -from selfdrive.hardware import PC +from system.hardware import PC if PC: SWAGLOG_DIR = os.path.join(str(Path.home()), ".comma", "log") diff --git a/selfdrive/test/helpers.py b/selfdrive/test/helpers.py index 5abc0d964f..cc8efd6e46 100644 --- a/selfdrive/test/helpers.py +++ b/selfdrive/test/helpers.py @@ -2,7 +2,7 @@ import os import time from functools import wraps -from selfdrive.hardware import PC +from system.hardware import PC from selfdrive.manager.process_config import managed_processes from selfdrive.version import training_version, terms_version diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py index b83629c76a..b466aa8193 100755 --- a/selfdrive/test/process_replay/model_replay.py +++ b/selfdrive/test/process_replay/model_replay.py @@ -11,7 +11,7 @@ from cereal.visionipc import VisionIpcServer, VisionStreamType from common.spinner import Spinner from common.timeout import Timeout from common.transformations.camera import get_view_frame_from_road_frame, tici_f_frame_size, tici_d_frame_size -from selfdrive.hardware import PC +from system.hardware import PC from selfdrive.manager.process_config import managed_processes from selfdrive.test.openpilotci import BASE_URL, get_url from selfdrive.test.process_replay.compare_logs import compare_logs, save_log diff --git a/selfdrive/test/process_replay/test_debayer.py b/selfdrive/test/process_replay/test_debayer.py index a17c71b602..cf939e3672 100755 --- a/selfdrive/test/process_replay/test_debayer.py +++ b/selfdrive/test/process_replay/test_debayer.py @@ -6,7 +6,7 @@ import numpy as np import pyopencl as cl # install with `PYOPENCL_CL_PRETEND_VERSION=2.0 pip install pyopencl` -from selfdrive.hardware import PC, TICI +from system.hardware import PC, TICI from common.basedir import BASEDIR from selfdrive.test.openpilotci import BASE_URL, get_url from selfdrive.version import get_commit diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py index d7a1f2a360..acc2fd90bc 100644 --- a/selfdrive/thermald/power_monitoring.py +++ b/selfdrive/thermald/power_monitoring.py @@ -4,7 +4,7 @@ from typing import Optional from cereal import log from common.params import Params, put_nonblocking from common.realtime import sec_since_boot -from selfdrive.hardware import HARDWARE +from system.hardware import HARDWARE from selfdrive.swaglog import cloudlog from selfdrive.statsd import statlog diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 4df1e072b8..84663e8eb0 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, AGNOS +from system.hardware import HARDWARE, TICI, AGNOS from selfdrive.loggerd.config import get_available_percent from selfdrive.statsd import statlog from selfdrive.swaglog import cloudlog diff --git a/selfdrive/timezoned.py b/selfdrive/timezoned.py index fc1ca92cdf..c1d5676db6 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 AGNOS +from system.hardware import AGNOS from selfdrive.swaglog import cloudlog diff --git a/selfdrive/ui/main.cc b/selfdrive/ui/main.cc index cffa459622..1eecd78b19 100644 --- a/selfdrive/ui/main.cc +++ b/selfdrive/ui/main.cc @@ -2,7 +2,7 @@ #include -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" #include "selfdrive/ui/qt/qt_window.h" #include "selfdrive/ui/qt/util.h" #include "selfdrive/ui/qt/window.h" diff --git a/selfdrive/ui/qt/api.cc b/selfdrive/ui/qt/api.cc index 94beb12186..84e1a4032e 100644 --- a/selfdrive/ui/qt/api.cc +++ b/selfdrive/ui/qt/api.cc @@ -14,7 +14,7 @@ #include "common/params.h" #include "common/util.h" -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" #include "selfdrive/ui/qt/util.h" namespace CommaApi { diff --git a/selfdrive/ui/qt/maps/map_helpers.cc b/selfdrive/ui/qt/maps/map_helpers.cc index 83576eb630..2b2c27418e 100644 --- a/selfdrive/ui/qt/maps/map_helpers.cc +++ b/selfdrive/ui/qt/maps/map_helpers.cc @@ -4,7 +4,7 @@ #include #include "common/params.h" -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" #include "selfdrive/ui/qt/api.h" QString get_mapbox_token() { diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index ac6f1f1ba0..175e6cf5a3 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -14,7 +14,7 @@ #include "common/params.h" #include "common/util.h" -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" #include "selfdrive/ui/qt/widgets/controls.h" #include "selfdrive/ui/qt/widgets/input.h" #include "selfdrive/ui/qt/widgets/scrollview.h" diff --git a/selfdrive/ui/qt/qt_window.h b/selfdrive/ui/qt/qt_window.h index 2c9a24e55b..02d127e7ff 100644 --- a/selfdrive/ui/qt/qt_window.h +++ b/selfdrive/ui/qt/qt_window.h @@ -12,7 +12,7 @@ #include #endif -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" const QString ASSET_PATH = ":/"; diff --git a/selfdrive/ui/qt/setup/setup.cc b/selfdrive/ui/qt/setup/setup.cc index 0da1752fed..10a2d05f34 100644 --- a/selfdrive/ui/qt/setup/setup.cc +++ b/selfdrive/ui/qt/setup/setup.cc @@ -11,7 +11,7 @@ #include #include "common/util.h" -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" #include "selfdrive/ui/qt/api.h" #include "selfdrive/ui/qt/qt_window.h" #include "selfdrive/ui/qt/offroad/networking.h" diff --git a/selfdrive/ui/qt/setup/updater.cc b/selfdrive/ui/qt/setup/updater.cc index b906b5739d..6e8189f4ba 100644 --- a/selfdrive/ui/qt/setup/updater.cc +++ b/selfdrive/ui/qt/setup/updater.cc @@ -2,7 +2,7 @@ #include #include -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" #include "selfdrive/ui/qt/util.h" #include "selfdrive/ui/qt/qt_window.h" #include "selfdrive/ui/qt/setup/updater.h" diff --git a/selfdrive/ui/qt/spinner.cc b/selfdrive/ui/qt/spinner.cc index e0b263227b..8f13576fb2 100644 --- a/selfdrive/ui/qt/spinner.cc +++ b/selfdrive/ui/qt/spinner.cc @@ -10,7 +10,7 @@ #include #include -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" #include "selfdrive/ui/qt/qt_window.h" #include "selfdrive/ui/qt/util.h" diff --git a/selfdrive/ui/qt/text.cc b/selfdrive/ui/qt/text.cc index 04fda35483..ac8e7bcd18 100644 --- a/selfdrive/ui/qt/text.cc +++ b/selfdrive/ui/qt/text.cc @@ -5,7 +5,7 @@ #include #include -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" #include "selfdrive/ui/qt/util.h" #include "selfdrive/ui/qt/qt_window.h" #include "selfdrive/ui/qt/widgets/scrollview.h" diff --git a/selfdrive/ui/qt/util.cc b/selfdrive/ui/qt/util.cc index 7ce7a267df..600885fb37 100644 --- a/selfdrive/ui/qt/util.cc +++ b/selfdrive/ui/qt/util.cc @@ -6,7 +6,7 @@ #include "common/params.h" #include "common/swaglog.h" -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" QString getVersion() { static QString version = QString::fromStdString(Params().get("Version")); diff --git a/selfdrive/ui/qt/widgets/input.cc b/selfdrive/ui/qt/widgets/input.cc index 1122faf4c7..8a5d492804 100644 --- a/selfdrive/ui/qt/widgets/input.cc +++ b/selfdrive/ui/qt/widgets/input.cc @@ -2,7 +2,7 @@ #include -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" #include "selfdrive/ui/qt/util.h" #include "selfdrive/ui/qt/qt_window.h" #include "selfdrive/ui/qt/widgets/scrollview.h" diff --git a/selfdrive/ui/qt/widgets/offroad_alerts.cc b/selfdrive/ui/qt/widgets/offroad_alerts.cc index 53f7eb677c..433193df5d 100644 --- a/selfdrive/ui/qt/widgets/offroad_alerts.cc +++ b/selfdrive/ui/qt/widgets/offroad_alerts.cc @@ -5,7 +5,7 @@ #include #include "common/util.h" -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" #include "selfdrive/ui/qt/widgets/scrollview.h" AbstractAlert::AbstractAlert(bool hasRebootBtn, QWidget *parent) : QFrame(parent) { diff --git a/selfdrive/ui/qt/widgets/ssh_keys.h b/selfdrive/ui/qt/widgets/ssh_keys.h index 0614e8c1d8..596d1d83b9 100644 --- a/selfdrive/ui/qt/widgets/ssh_keys.h +++ b/selfdrive/ui/qt/widgets/ssh_keys.h @@ -2,7 +2,7 @@ #include -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" #include "selfdrive/ui/qt/widgets/controls.h" // SSH enable toggle diff --git a/selfdrive/ui/qt/window.cc b/selfdrive/ui/qt/window.cc index fb21f6f7af..d9613df0d0 100644 --- a/selfdrive/ui/qt/window.cc +++ b/selfdrive/ui/qt/window.cc @@ -2,7 +2,7 @@ #include -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" MainWindow::MainWindow(QWidget *parent) : QWidget(parent) { main_layout = new QStackedLayout(this); diff --git a/selfdrive/ui/replay/replay.cc b/selfdrive/ui/replay/replay.cc index 4036086e1b..5eb7469c92 100644 --- a/selfdrive/ui/replay/replay.cc +++ b/selfdrive/ui/replay/replay.cc @@ -7,7 +7,7 @@ #include "cereal/services.h" #include "common/params.h" #include "common/timing.h" -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" #include "selfdrive/ui/replay/util.h" Replay::Replay(QString route, QStringList allow, QStringList block, SubMaster *sm_, uint32_t flags, QString data_dir, QObject *parent) diff --git a/selfdrive/ui/replay/route.cc b/selfdrive/ui/replay/route.cc index ad93263ae9..3d595141cd 100644 --- a/selfdrive/ui/replay/route.cc +++ b/selfdrive/ui/replay/route.cc @@ -9,7 +9,7 @@ #include -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" #include "selfdrive/ui/qt/api.h" #include "selfdrive/ui/replay/replay.h" #include "selfdrive/ui/replay/util.h" diff --git a/selfdrive/ui/soundd/sound.h b/selfdrive/ui/soundd/sound.h index 82b360fd38..7e009d28ad 100644 --- a/selfdrive/ui/soundd/sound.h +++ b/selfdrive/ui/soundd/sound.h @@ -2,7 +2,7 @@ #include #include -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" #include "selfdrive/ui/ui.h" const std::tuple sound_list[] = { diff --git a/selfdrive/ui/tests/test_soundd.py b/selfdrive/ui/tests/test_soundd.py index cba0f6dbd1..8cc9215b74 100755 --- a/selfdrive/ui/tests/test_soundd.py +++ b/selfdrive/ui/tests/test_soundd.py @@ -8,7 +8,7 @@ import cereal.messaging as messaging from selfdrive.test.helpers import phone_only, with_processes # TODO: rewrite for unittest from common.realtime import DT_CTRL -from selfdrive.hardware import HARDWARE +from system.hardware import HARDWARE AudibleAlert = car.CarControl.HUDControl.AudibleAlert diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index c1af4ed6f4..4d1e1ab746 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -10,7 +10,7 @@ #include "common/swaglog.h" #include "common/util.h" #include "common/watchdog.h" -#include "selfdrive/hardware/hw.h" +#include "system/hardware/hw.h" #define BACKLIGHT_DT 0.05 #define BACKLIGHT_TS 10.00 diff --git a/selfdrive/updated.py b/selfdrive/updated.py index f835522cdb..33e2d5d418 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 AGNOS, HARDWARE +from system.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 @@ -265,7 +265,7 @@ def finalize_update(wait_helper: WaitTimeHelper) -> None: def handle_agnos_update(wait_helper: WaitTimeHelper) -> None: - from selfdrive.hardware.tici.agnos import flash_agnos_update, get_target_slot_number + from system.hardware.tici.agnos import flash_agnos_update, get_target_slot_number cur_version = HARDWARE.get_os_version() updated_version = run(["bash", "-c", r"unset AGNOS_VERSION && source launch_env.sh && \ @@ -281,7 +281,7 @@ def handle_agnos_update(wait_helper: WaitTimeHelper) -> None: cloudlog.info(f"Beginning background installation for AGNOS {updated_version}") set_offroad_alert("Offroad_NeosUpdate", True) - manifest_path = os.path.join(OVERLAY_MERGED, "selfdrive/hardware/tici/agnos.json") + manifest_path = os.path.join(OVERLAY_MERGED, "system/hardware/tici/agnos.json") target_slot_number = get_target_slot_number() flash_agnos_update(manifest_path, target_slot_number, cloudlog) set_offroad_alert("Offroad_NeosUpdate", False) diff --git a/selfdrive/hardware/pc/__init__.py b/system/__init__.py similarity index 100% rename from selfdrive/hardware/pc/__init__.py rename to system/__init__.py diff --git a/selfdrive/hardware/.gitignore b/system/hardware/.gitignore similarity index 100% rename from selfdrive/hardware/.gitignore rename to system/hardware/.gitignore diff --git a/selfdrive/hardware/__init__.py b/system/hardware/__init__.py similarity index 56% rename from selfdrive/hardware/__init__.py rename to system/hardware/__init__.py index a1bf2e912f..780a20f9c0 100644 --- a/selfdrive/hardware/__init__.py +++ b/system/hardware/__init__.py @@ -1,9 +1,9 @@ import os from typing import cast -from selfdrive.hardware.base import HardwareBase -from selfdrive.hardware.tici.hardware import Tici -from selfdrive.hardware.pc.hardware import Pc +from system.hardware.base import HardwareBase +from system.hardware.tici.hardware import Tici +from system.hardware.pc.hardware import Pc TICI = os.path.isfile('/TICI') AGNOS = TICI diff --git a/selfdrive/hardware/base.h b/system/hardware/base.h similarity index 100% rename from selfdrive/hardware/base.h rename to system/hardware/base.h diff --git a/selfdrive/hardware/base.py b/system/hardware/base.py similarity index 100% rename from selfdrive/hardware/base.py rename to system/hardware/base.py diff --git a/selfdrive/hardware/hw.h b/system/hardware/hw.h similarity index 92% rename from selfdrive/hardware/hw.h rename to system/hardware/hw.h index 364a1c5c33..f50e94abe1 100644 --- a/selfdrive/hardware/hw.h +++ b/system/hardware/hw.h @@ -1,10 +1,10 @@ #pragma once -#include "selfdrive/hardware/base.h" +#include "system/hardware/base.h" #include "common/util.h" #if QCOM2 -#include "selfdrive/hardware/tici/hardware.h" +#include "system/hardware/tici/hardware.h" #define Hardware HardwareTici #else class HardwarePC : public HardwareNone { diff --git a/selfdrive/hardware/tici/__init__.py b/system/hardware/pc/__init__.py similarity index 100% rename from selfdrive/hardware/tici/__init__.py rename to system/hardware/pc/__init__.py diff --git a/selfdrive/hardware/pc/hardware.py b/system/hardware/pc/hardware.py similarity index 96% rename from selfdrive/hardware/pc/hardware.py rename to system/hardware/pc/hardware.py index 2f5db925a9..b2c4a4343b 100644 --- a/selfdrive/hardware/pc/hardware.py +++ b/system/hardware/pc/hardware.py @@ -1,7 +1,7 @@ import random from cereal import log -from selfdrive.hardware.base import HardwareBase, ThermalConfig +from system.hardware.base import HardwareBase, ThermalConfig NetworkType = log.DeviceState.NetworkType NetworkStrength = log.DeviceState.NetworkStrength diff --git a/system/hardware/tici/__init__.py b/system/hardware/tici/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/hardware/tici/agnos.json b/system/hardware/tici/agnos.json similarity index 100% rename from selfdrive/hardware/tici/agnos.json rename to system/hardware/tici/agnos.json diff --git a/selfdrive/hardware/tici/agnos.py b/system/hardware/tici/agnos.py similarity index 100% rename from selfdrive/hardware/tici/agnos.py rename to system/hardware/tici/agnos.py diff --git a/selfdrive/hardware/tici/amplifier.py b/system/hardware/tici/amplifier.py similarity index 100% rename from selfdrive/hardware/tici/amplifier.py rename to system/hardware/tici/amplifier.py diff --git a/selfdrive/hardware/tici/hardware.h b/system/hardware/tici/hardware.h similarity index 97% rename from selfdrive/hardware/tici/hardware.h rename to system/hardware/tici/hardware.h index b11a0a5bb0..dcccb9f3d1 100644 --- a/selfdrive/hardware/tici/hardware.h +++ b/system/hardware/tici/hardware.h @@ -5,7 +5,7 @@ #include "common/params.h" #include "common/util.h" -#include "selfdrive/hardware/base.h" +#include "system/hardware/base.h" class HardwareTici : public HardwareNone { public: diff --git a/selfdrive/hardware/tici/hardware.py b/system/hardware/tici/hardware.py similarity index 98% rename from selfdrive/hardware/tici/hardware.py rename to system/hardware/tici/hardware.py index 0a92340598..a69d3eb743 100644 --- a/selfdrive/hardware/tici/hardware.py +++ b/system/hardware/tici/hardware.py @@ -9,10 +9,10 @@ from pathlib import Path from cereal import log from common.gpio import gpio_set, gpio_init -from selfdrive.hardware.base import HardwareBase, ThermalConfig -from selfdrive.hardware.tici import iwlist -from selfdrive.hardware.tici.pins import GPIO -from selfdrive.hardware.tici.amplifier import Amplifier +from system.hardware.base import HardwareBase, ThermalConfig +from system.hardware.tici import iwlist +from system.hardware.tici.pins import GPIO +from system.hardware.tici.amplifier import Amplifier NM = 'org.freedesktop.NetworkManager' NM_CON_ACT = NM + '.Connection.Active' diff --git a/selfdrive/hardware/tici/iwlist.py b/system/hardware/tici/iwlist.py similarity index 100% rename from selfdrive/hardware/tici/iwlist.py rename to system/hardware/tici/iwlist.py diff --git a/selfdrive/hardware/tici/pins.py b/system/hardware/tici/pins.py similarity index 100% rename from selfdrive/hardware/tici/pins.py rename to system/hardware/tici/pins.py diff --git a/selfdrive/hardware/tici/power_draw_test.py b/system/hardware/tici/power_draw_test.py similarity index 97% rename from selfdrive/hardware/tici/power_draw_test.py rename to system/hardware/tici/power_draw_test.py index 1af51fc018..bde92ae4a5 100755 --- a/selfdrive/hardware/tici/power_draw_test.py +++ b/system/hardware/tici/power_draw_test.py @@ -2,8 +2,8 @@ import os import time import numpy as np -from selfdrive.hardware.tici.hardware import Tici -from selfdrive.hardware.tici.pins import GPIO +from system.hardware.tici.hardware import Tici +from system.hardware.tici.pins import GPIO from common.gpio import gpio_init, gpio_set def read_power(): diff --git a/selfdrive/hardware/tici/power_monitor.py b/system/hardware/tici/power_monitor.py similarity index 100% rename from selfdrive/hardware/tici/power_monitor.py rename to system/hardware/tici/power_monitor.py diff --git a/selfdrive/hardware/tici/precise_power_measure.py b/system/hardware/tici/precise_power_measure.py similarity index 77% rename from selfdrive/hardware/tici/precise_power_measure.py rename to system/hardware/tici/precise_power_measure.py index c66936aef8..5d68851367 100755 --- a/selfdrive/hardware/tici/precise_power_measure.py +++ b/system/hardware/tici/precise_power_measure.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 import numpy as np -from selfdrive.hardware.tici.power_monitor import sample_power +from system.hardware.tici.power_monitor import sample_power if __name__ == '__main__': print("measuring for 5 seconds") diff --git a/selfdrive/hardware/tici/restart_modem.sh b/system/hardware/tici/restart_modem.sh similarity index 100% rename from selfdrive/hardware/tici/restart_modem.sh rename to system/hardware/tici/restart_modem.sh diff --git a/selfdrive/hardware/tici/test_agnos_updater.py b/system/hardware/tici/test_agnos_updater.py similarity index 100% rename from selfdrive/hardware/tici/test_agnos_updater.py rename to system/hardware/tici/test_agnos_updater.py diff --git a/selfdrive/hardware/tici/test_power_draw.py b/system/hardware/tici/test_power_draw.py similarity index 92% rename from selfdrive/hardware/tici/test_power_draw.py rename to system/hardware/tici/test_power_draw.py index ab2d691a09..31b8471328 100755 --- a/selfdrive/hardware/tici/test_power_draw.py +++ b/system/hardware/tici/test_power_draw.py @@ -4,8 +4,8 @@ import time import math from dataclasses import dataclass -from selfdrive.hardware import HARDWARE, TICI -from selfdrive.hardware.tici.power_monitor import get_power +from system.hardware import HARDWARE, TICI +from system.hardware.tici.power_monitor import get_power from selfdrive.manager.process_config import managed_processes from selfdrive.manager.manager import manager_cleanup diff --git a/selfdrive/hardware/tici/updater b/system/hardware/tici/updater similarity index 100% rename from selfdrive/hardware/tici/updater rename to system/hardware/tici/updater diff --git a/tools/lib/auth_config.py b/tools/lib/auth_config.py index 1699d94e53..7952fee495 100644 --- a/tools/lib/auth_config.py +++ b/tools/lib/auth_config.py @@ -1,7 +1,7 @@ import json import os from common.file_helpers import mkdirs_exists_ok -from selfdrive.hardware import PC +from system.hardware import PC class MissingAuthConfigError(Exception): From 6856c2d4efda54708be3790bb094b5c377dee575 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sat, 11 Jun 2022 17:43:34 -0700 Subject: [PATCH 46/93] jenkins: remove unnecessary workstation clean --- Jenkinsfile | 5 ----- 1 file changed, 5 deletions(-) diff --git a/Jenkinsfile b/Jenkinsfile index 250893d1af..9d1ab33761 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -154,11 +154,6 @@ pipeline { } } - post { - always { - cleanWs() - } - } } } } From 83c5d2ede6c058a8eb723933a45bd3abc810bf51 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sat, 11 Jun 2022 17:56:59 -0700 Subject: [PATCH 47/93] bump cereal --- cereal | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cereal b/cereal index a197e38c0c..78870ba056 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit a197e38c0cef00ececf4c7500438d37659d9199e +Subproject commit 78870ba056174352218988610e5010fde4eca956 From 9cda2d63380dc9b554212ba15e82ca0621ab1945 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sat, 11 Jun 2022 18:43:24 -0700 Subject: [PATCH 48/93] set AGNOS from /AGNOS file --- system/hardware/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/hardware/__init__.py b/system/hardware/__init__.py index 780a20f9c0..77bb0e5e2a 100644 --- a/system/hardware/__init__.py +++ b/system/hardware/__init__.py @@ -6,7 +6,7 @@ from system.hardware.tici.hardware import Tici from system.hardware.pc.hardware import Pc TICI = os.path.isfile('/TICI') -AGNOS = TICI +AGNOS = os.path.isfile('/AGNOS') PC = not TICI From fbd98b0e541b89f477061bab34dfc41495ac9399 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sat, 11 Jun 2022 22:51:09 -0700 Subject: [PATCH 49/93] CI: add build job for latest Ubuntu (#24637) * CI: add build job for latest Ubuntu * source * source env * scons cache * cache pyenv * fix key * source --- .github/workflows/selfdrive_tests.yaml | 2 +- .github/workflows/tools_tests.yaml | 40 ++++++++++++++++++++++++++ 2 files changed, 41 insertions(+), 1 deletion(-) diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index 567fc05aa5..5bd9acc200 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -94,7 +94,7 @@ jobs: run: | ${{ env.RUN }} "scons -j$(nproc) --extras --test && \ rm -rf /tmp/scons_cache/* && \ - scons -j$(nproc) --cache-populate" + scons -j$(nproc) --extras --test --cache-populate" #build_mac: # name: build macos diff --git a/.github/workflows/tools_tests.yaml b/.github/workflows/tools_tests.yaml index 1e4ce7a4ae..b2433fba8f 100644 --- a/.github/workflows/tools_tests.yaml +++ b/.github/workflows/tools_tests.yaml @@ -21,6 +21,46 @@ env: GITHUB_REPOSITORY -e GITHUB_RUN_ID -v /tmp/comma_download_cache:/tmp/comma_download_cache $BASE_IMAGE /bin/sh -c jobs: + build_latest_ubuntu: + name: build latest ubuntu + runs-on: ubuntu-20.04 + timeout-minutes: 60 + steps: + - uses: actions/checkout@v3 + with: + submodules: true + - name: Cache pyenv + id: ubuntu-latest-pyenv + uses: actions/cache@v3 + with: + path: | + ~/.pyenv + ~/.local/share/virtualenvs/ + key: ubuntu-latest-python-${{ hashFiles('tools/ubuntu_setup.sh') }}- + - name: Cache scons + id: ubuntu-latest-scons + uses: actions/cache@v3 + with: + path: /tmp/scons_cache + key: ubuntu-latest-scons-${{ hashFiles('.github/workflows/tools_test.yaml') }}- + restore-keys: | + ubuntu-latest-scons-${{ hashFiles('.github/workflows/tools_test.yaml') }}- + ubuntu-latest-scons- + + - name: tools/ubuntu_setup.sh + run: | + source tools/openpilot_env.sh + tools/ubuntu_setup.sh + - name: Build openpilot + run: | + source tools/openpilot_env.sh + pipenv run scons -j$(nproc) --extras --test + - name: Cleanup scons cache + run: | + source tools/openpilot_env.sh + rm -rf /tmp/scons_cache/* + pipenv run scons -j$(nproc) --extras --test --cache-populate + plotjuggler: name: plotjuggler runs-on: ubuntu-20.04 From 0fce5d90459b77bf2cfa70f55f322f0e1fb8d01c Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sat, 11 Jun 2022 23:19:27 -0700 Subject: [PATCH 50/93] Move a bunch of stuff to system/ part 3 (#24829) * move swaglog.py * timezoned * logmessaged * version.py * fix linter --- .pylintrc | 2 +- common/api/__init__.py | 2 +- docs/conf.py | 6 ++++-- release/files_common | 8 ++++---- release/files_tici | 2 +- selfdrive/athena/athenad.py | 4 ++-- selfdrive/athena/manage_athenad.py | 4 ++-- selfdrive/athena/registration.py | 2 +- selfdrive/athena/tests/test_athenad.py | 2 +- selfdrive/boardd/pandad.py | 2 +- selfdrive/boardd/tests/boardd_old.py | 2 +- selfdrive/car/car_helpers.py | 4 ++-- selfdrive/car/disable_ecu.py | 2 +- selfdrive/car/fw_versions.py | 2 +- selfdrive/car/isotp_parallel_query.py | 2 +- selfdrive/car/mock/interface.py | 2 +- selfdrive/car/vin.py | 2 +- selfdrive/controls/controlsd.py | 2 +- selfdrive/controls/lib/events.py | 2 +- selfdrive/controls/lib/lane_planner.py | 2 +- selfdrive/controls/lib/lateral_planner.py | 2 +- .../controls/lib/longitudinal_mpc_lib/long_mpc.py | 2 +- selfdrive/controls/lib/longitudinal_planner.py | 2 +- selfdrive/controls/plannerd.py | 2 +- selfdrive/controls/radard.py | 2 +- selfdrive/debug/disable_ecu.py | 2 +- selfdrive/locationd/calibrationd.py | 2 +- selfdrive/locationd/laikad.py | 2 +- selfdrive/locationd/models/car_kf.py | 2 +- selfdrive/locationd/paramsd.py | 2 +- selfdrive/loggerd/deleter.py | 2 +- selfdrive/loggerd/tests/test_loggerd.py | 2 +- selfdrive/loggerd/tests/test_uploader.py | 2 +- selfdrive/loggerd/uploader.py | 2 +- selfdrive/manager/build.py | 4 ++-- selfdrive/manager/manager.py | 4 ++-- selfdrive/manager/process.py | 2 +- selfdrive/manager/process_config.py | 11 ++++++----- selfdrive/navd/navd.py | 2 +- selfdrive/sensord/rawgps/rawgpsd.py | 2 +- selfdrive/sentry.py | 4 ++-- selfdrive/statsd.py | 4 ++-- selfdrive/test/helpers.py | 2 +- selfdrive/test/process_replay/model_replay.py | 2 +- selfdrive/test/process_replay/test_debayer.py | 2 +- selfdrive/test/process_replay/test_processes.py | 2 +- selfdrive/test/test_onroad.py | 2 +- selfdrive/thermald/fan_controller.py | 2 +- selfdrive/thermald/power_monitoring.py | 2 +- selfdrive/thermald/thermald.py | 4 ++-- selfdrive/tombstoned.py | 4 ++-- selfdrive/updated.py | 4 ++-- {selfdrive => system}/logmessaged.py | 2 +- {selfdrive => system}/swaglog.py | 0 {selfdrive => system}/timezoned.py | 2 +- {selfdrive => system}/version.py | 2 +- 56 files changed, 76 insertions(+), 73 deletions(-) rename {selfdrive => system}/logmessaged.py (95%) rename {selfdrive => system}/swaglog.py (100%) rename {selfdrive => system}/timezoned.py (98%) rename {selfdrive => system}/version.py (99%) diff --git a/.pylintrc b/.pylintrc index 27539b743c..58988c5d74 100644 --- a/.pylintrc +++ b/.pylintrc @@ -3,7 +3,7 @@ # A comma-separated list of package or module names from where C extensions may # be loaded. Extensions are loading into the active Python interpreter and may # run arbitrary code -extension-pkg-whitelist=scipy,cereal.messaging.messaging_pyx,PyQt5 +extension-pkg-whitelist=scipy,cereal.messaging.messaging_pyx,PyQt5,av # Add files or directories to the blacklist. They should be base names, not # paths. diff --git a/common/api/__init__.py b/common/api/__init__.py index fd2e70910e..c1fa635bd6 100644 --- a/common/api/__init__.py +++ b/common/api/__init__.py @@ -3,7 +3,7 @@ import os import requests from datetime import datetime, timedelta from common.basedir import PERSIST -from selfdrive.version import get_version +from system.version import get_version API_HOST = os.getenv('API_HOST', 'https://api.commadotai.com') diff --git a/docs/conf.py b/docs/conf.py index c11d17455d..fea921de1f 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -14,10 +14,12 @@ # documentation root, use os.path.abspath to make it absolute, like shown here. # import os -from os.path import exists import sys -from selfdrive.version import get_version +from os.path import exists + from common.basedir import BASEDIR +from system.version import get_version + sys.path.insert(0, os.path.abspath('.')) sys.path.insert(0, os.path.abspath('..')) diff --git a/release/files_common b/release/files_common index e89163aba0..76d440c402 100644 --- a/release/files_common +++ b/release/files_common @@ -61,17 +61,17 @@ release/* tools/lib/* tools/joystick/* -selfdrive/version.py - selfdrive/__init__.py selfdrive/sentry.py -selfdrive/swaglog.py -selfdrive/logmessaged.py selfdrive/tombstoned.py selfdrive/updated.py selfdrive/rtshield.py selfdrive/statsd.py +system/logmessaged.py +system/swaglog.py +system/version.py + selfdrive/athena/__init__.py selfdrive/athena/athenad.py selfdrive/athena/manage_athenad.py diff --git a/release/files_tici b/release/files_tici index ee1ac63c34..485a898799 100644 --- a/release/files_tici +++ b/release/files_tici @@ -2,7 +2,7 @@ third_party/snpe/larch64** third_party/snpe/aarch64-ubuntu-gcc7.5/* third_party/mapbox-gl-native-qt/include/* -selfdrive/timezoned.py +system/timezoned.py selfdrive/assets/navigation/* selfdrive/assets/training_wide/* diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index 0dc7d704a8..26220dfa99 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -36,8 +36,8 @@ from system.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 -from selfdrive.swaglog import SWAGLOG_DIR, cloudlog -from selfdrive.version import get_commit, get_origin, get_short_branch, get_version +from system.swaglog import SWAGLOG_DIR, cloudlog +from system.version import get_commit, get_origin, get_short_branch, get_version ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai') HANDLER_THREADS = int(os.getenv('HANDLER_THREADS', "4")) diff --git a/selfdrive/athena/manage_athenad.py b/selfdrive/athena/manage_athenad.py index 58ad58310f..6bbb03a799 100755 --- a/selfdrive/athena/manage_athenad.py +++ b/selfdrive/athena/manage_athenad.py @@ -5,8 +5,8 @@ from multiprocessing import Process from common.params import Params from selfdrive.manager.process import launcher -from selfdrive.swaglog import cloudlog -from selfdrive.version import get_version, is_dirty +from system.swaglog import cloudlog +from system.version import get_version, is_dirty ATHENA_MGR_PID_PARAM = "AthenadPid" diff --git a/selfdrive/athena/registration.py b/selfdrive/athena/registration.py index c44ca4c28b..32bc92059c 100755 --- a/selfdrive/athena/registration.py +++ b/selfdrive/athena/registration.py @@ -12,7 +12,7 @@ from common.spinner import Spinner from common.basedir import PERSIST from selfdrive.controls.lib.alertmanager import set_offroad_alert from system.hardware import HARDWARE, PC -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog UNREGISTERED_DONGLE_ID = "UnregisteredDevice" diff --git a/selfdrive/athena/tests/test_athenad.py b/selfdrive/athena/tests/test_athenad.py index a0b308c216..382b549c1b 100755 --- a/selfdrive/athena/tests/test_athenad.py +++ b/selfdrive/athena/tests/test_athenad.py @@ -16,7 +16,7 @@ from unittest import mock from websocket import ABNF from websocket._exceptions import WebSocketConnectionClosedException -from selfdrive import swaglog +from system import swaglog from selfdrive.athena import athenad from selfdrive.athena.athenad import MAX_RETRY_COUNT, dispatcher from selfdrive.athena.tests.helpers import MockWebsocket, MockParams, MockApi, EchoSocket, with_http_server diff --git a/selfdrive/boardd/pandad.py b/selfdrive/boardd/pandad.py index 51bfb92dd9..54a28a6782 100755 --- a/selfdrive/boardd/pandad.py +++ b/selfdrive/boardd/pandad.py @@ -11,7 +11,7 @@ from panda import DEFAULT_FW_FN, DEFAULT_H7_FW_FN, MCU_TYPE_H7, Panda, PandaDFU from common.basedir import BASEDIR from common.params import Params from system.hardware import HARDWARE -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog def get_expected_signature(panda: Panda) -> bytes: diff --git a/selfdrive/boardd/tests/boardd_old.py b/selfdrive/boardd/tests/boardd_old.py index 5e740fbcd8..fad29f6f34 100755 --- a/selfdrive/boardd/tests/boardd_old.py +++ b/selfdrive/boardd/tests/boardd_old.py @@ -13,7 +13,7 @@ import time import cereal.messaging as messaging from common.realtime import Ratekeeper -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog from selfdrive.boardd.boardd import can_capnp_to_can_list from cereal import car diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 7863f0d882..3bfd99f88d 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -4,12 +4,12 @@ from typing import Dict, List from cereal import car from common.params import Params from common.basedir import BASEDIR -from selfdrive.version import is_comma_remote, is_tested_branch +from system.version import is_comma_remote, is_tested_branch from selfdrive.car.interfaces import get_interface_attr from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars from selfdrive.car.vin import get_vin, VIN_UNKNOWN from selfdrive.car.fw_versions import get_fw_versions, match_fw_to_car -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog import cereal.messaging as messaging from selfdrive.car import gen_empty_fingerprint diff --git a/selfdrive/car/disable_ecu.py b/selfdrive/car/disable_ecu.py index 3a06cc06f1..ac5c6c9f8f 100644 --- a/selfdrive/car/disable_ecu.py +++ b/selfdrive/car/disable_ecu.py @@ -1,5 +1,5 @@ from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog EXT_DIAG_REQUEST = b'\x10\x03' EXT_DIAG_RESPONSE = b'\x50\x03' diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index 69b7c5f452..c7253d794d 100755 --- a/selfdrive/car/fw_versions.py +++ b/selfdrive/car/fw_versions.py @@ -12,7 +12,7 @@ from selfdrive.car.interfaces import get_interface_attr from selfdrive.car.fingerprints import FW_VERSIONS from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery from selfdrive.car.toyota.values import CAR as TOYOTA -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog Ecu = car.CarParams.Ecu diff --git a/selfdrive/car/isotp_parallel_query.py b/selfdrive/car/isotp_parallel_query.py index dc79babb13..0e807512cf 100644 --- a/selfdrive/car/isotp_parallel_query.py +++ b/selfdrive/car/isotp_parallel_query.py @@ -4,7 +4,7 @@ from functools import partial from typing import Optional import cereal.messaging as messaging -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog from selfdrive.boardd.boardd import can_list_to_can_capnp from panda.python.uds import CanClient, IsoTpMessage, FUNCTIONAL_ADDRS, get_rx_addr_for_tx_addr diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index 65e886751d..715850c227 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -2,7 +2,7 @@ import math from cereal import car from common.conversions import Conversions as CV -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog import cereal.messaging as messaging from selfdrive.car import gen_empty_fingerprint, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase diff --git a/selfdrive/car/vin.py b/selfdrive/car/vin.py index 2c5b623b06..7413c3f235 100755 --- a/selfdrive/car/vin.py +++ b/selfdrive/car/vin.py @@ -6,7 +6,7 @@ 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 +from system.swaglog import cloudlog OBD_VIN_REQUEST = b'\x09\x02' OBD_VIN_RESPONSE = b'\x49\x02\x01' diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 31807b0187..033072aa73 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -11,7 +11,7 @@ from common.params import Params, put_nonblocking import cereal.messaging as messaging from common.conversions import Conversions as CV from panda import ALTERNATIVE_EXPERIENCE -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 7ca05cc744..cc63d4995d 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -8,7 +8,7 @@ import cereal.messaging as messaging from common.conversions import Conversions as CV from common.realtime import DT_CTRL from selfdrive.locationd.calibrationd import MIN_SPEED_FILTER -from selfdrive.version import get_short_branch +from system.version import get_short_branch AlertSize = log.ControlsState.AlertSize AlertStatus = log.ControlsState.AlertStatus diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index 9bbad59084..1facb66d63 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -3,7 +3,7 @@ from cereal import log from common.filter_simple import FirstOrderFilter from common.numpy_fast import interp from common.realtime import DT_MDL -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog TRAJECTORY_SIZE = 33 diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index f3f59ee308..ec21c16e91 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -1,7 +1,7 @@ import numpy as np from common.realtime import sec_since_boot, DT_MDL from common.numpy_fast import interp -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc from selfdrive.controls.lib.drive_helpers import CONTROL_N, MPC_COST_LAT, LAT_MPC_N, CAR_ROTATION_RADIUS from selfdrive.controls.lib.lane_planner import LanePlanner, TRAJECTORY_SIZE diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index b99ee9e9ac..bc0fc9fea6 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -4,7 +4,7 @@ import numpy as np from common.realtime import sec_since_boot from common.numpy_fast import clip, interp -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog from selfdrive.modeld.constants import index_function from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 0d21c519a3..d4a6aaef8f 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -12,7 +12,7 @@ from selfdrive.controls.lib.longcontrol import LongCtrlState from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog LON_MPC_STEP = 0.2 # first step is 0.2s AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 083aeb79cc..06807b2a5c 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -2,7 +2,7 @@ from cereal import car from common.params import Params from common.realtime import Priority, config_realtime_process -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog from selfdrive.controls.lib.longitudinal_planner import Planner from selfdrive.controls.lib.lateral_planner import LateralPlanner import cereal.messaging as messaging diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 0e514193dc..b2c9914457 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -10,7 +10,7 @@ from common.params import Params 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 system.swaglog import cloudlog class KalmanParams(): diff --git a/selfdrive/debug/disable_ecu.py b/selfdrive/debug/disable_ecu.py index f0faf40017..af007207eb 100644 --- a/selfdrive/debug/disable_ecu.py +++ b/selfdrive/debug/disable_ecu.py @@ -3,7 +3,7 @@ import traceback import cereal.messaging as messaging from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog EXT_DIAG_REQUEST = b'\x10\x03' EXT_DIAG_RESPONSE = b'\x50\x03' diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index d7bf36fb26..e092c939ae 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -20,7 +20,7 @@ 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.swaglog import cloudlog +from system.swaglog import cloudlog MIN_SPEED_FILTER = 15 * CV.MPH_TO_MS MAX_VEL_ANGLE_STD = np.radians(0.25) diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 4ec159cd25..01457f4681 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -17,7 +17,7 @@ from selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind from selfdrive.locationd.models.gnss_kf import GNSSKalman from selfdrive.locationd.models.gnss_kf import States as GStates import common.transformations.coordinates as coord -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog MAX_TIME_GAP = 10 diff --git a/selfdrive/locationd/models/car_kf.py b/selfdrive/locationd/models/car_kf.py index 75534efa5a..fe7d2e650b 100755 --- a/selfdrive/locationd/models/car_kf.py +++ b/selfdrive/locationd/models/car_kf.py @@ -7,7 +7,7 @@ import numpy as np from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY from selfdrive.locationd.models.constants import ObservationKind -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog from rednose.helpers.kalmanfilter import KalmanFilter diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 0e83728e5c..ae67dc28ab 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -10,7 +10,7 @@ from common.realtime import config_realtime_process, DT_MDL from common.numpy_fast import clip from selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States from selfdrive.locationd.models.constants import GENERATED_DIR -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL # Max 20 deg/s diff --git a/selfdrive/loggerd/deleter.py b/selfdrive/loggerd/deleter.py index d745e91fb5..5606288024 100644 --- a/selfdrive/loggerd/deleter.py +++ b/selfdrive/loggerd/deleter.py @@ -2,7 +2,7 @@ import os import shutil import threading -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog from selfdrive.loggerd.config import ROOT, get_available_bytes, get_available_percent from selfdrive.loggerd.uploader import listdir_by_creation diff --git a/selfdrive/loggerd/tests/test_loggerd.py b/selfdrive/loggerd/tests/test_loggerd.py index 54f7aaaa2b..9dbc7ac332 100755 --- a/selfdrive/loggerd/tests/test_loggerd.py +++ b/selfdrive/loggerd/tests/test_loggerd.py @@ -17,7 +17,7 @@ from common.params import Params from common.timeout import Timeout from selfdrive.loggerd.config import ROOT from selfdrive.manager.process_config import managed_processes -from selfdrive.version import get_version +from system.version import get_version from tools.lib.logreader import LogReader from cereal.visionipc import VisionIpcServer, VisionStreamType from common.transformations.camera import tici_f_frame_size, tici_d_frame_size, tici_e_frame_size diff --git a/selfdrive/loggerd/tests/test_uploader.py b/selfdrive/loggerd/tests/test_uploader.py index b8c01776ae..6090bbe2aa 100755 --- a/selfdrive/loggerd/tests/test_uploader.py +++ b/selfdrive/loggerd/tests/test_uploader.py @@ -6,7 +6,7 @@ import unittest import logging import json -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog import selfdrive.loggerd.uploader as uploader from selfdrive.loggerd.tests.loggerd_tests_common import UploaderTestCase diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index 9de1c421bd..f97bafecb9 100644 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -18,7 +18,7 @@ from common.realtime import set_core_affinity from system.hardware import TICI from selfdrive.loggerd.xattr_cache import getxattr, setxattr from selfdrive.loggerd.config import ROOT -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog NetworkType = log.DeviceState.NetworkType UPLOAD_ATTR_NAME = 'user.upload' diff --git a/selfdrive/manager/build.py b/selfdrive/manager/build.py index f0a5aec927..10b4c0a4b8 100755 --- a/selfdrive/manager/build.py +++ b/selfdrive/manager/build.py @@ -11,8 +11,8 @@ from common.basedir import BASEDIR from common.spinner import Spinner from common.text_window import TextWindow from system.hardware import AGNOS -from selfdrive.swaglog import cloudlog, add_file_handler -from selfdrive.version import is_dirty +from system.swaglog import cloudlog, add_file_handler +from system.version import is_dirty MAX_CACHE_SIZE = 4e9 if "CI" in os.environ else 2e9 CACHE_DIR = Path("/data/scons_cache" if AGNOS else "/tmp/scons_cache") diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 94af397c7d..140c7f1d44 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -18,8 +18,8 @@ from selfdrive.manager.helpers import unblock_stdout from selfdrive.manager.process import ensure_running from selfdrive.manager.process_config import managed_processes from selfdrive.athena.registration import register, UNREGISTERED_DONGLE_ID -from selfdrive.swaglog import cloudlog, add_file_handler -from selfdrive.version import is_dirty, get_commit, get_version, get_origin, get_short_branch, \ +from system.swaglog import cloudlog, add_file_handler +from system.version import is_dirty, get_commit, get_version, get_origin, get_short_branch, \ terms_version, training_version diff --git a/selfdrive/manager/process.py b/selfdrive/manager/process.py index 22f55f3b36..dabfbe4ee0 100644 --- a/selfdrive/manager/process.py +++ b/selfdrive/manager/process.py @@ -16,7 +16,7 @@ from cereal import car from common.basedir import BASEDIR from common.params import Params from common.realtime import sec_since_boot -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog from system.hardware import HARDWARE from cereal import log diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 443ebbb419..b3efe7e2de 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -18,16 +18,19 @@ def logging(started, params, CP: car.CarParams) -> bool: return started and run procs = [ + NativeProcess("clocksd", "system/clocksd", ["./clocksd"]), + NativeProcess("logcatd", "system/logcatd", ["./logcatd"]), + NativeProcess("proclogd", "system/proclogd", ["./proclogd"]), + PythonProcess("logmessaged", "system.logmessaged", offroad=True), + PythonProcess("timezoned", "system.timezoned", enabled=not PC, offroad=True), + DaemonProcess("manage_athenad", "selfdrive.athena.manage_athenad", "AthenadPid"), # due to qualcomm kernel bugs SIGKILLing camerad sometimes causes page table corruption NativeProcess("camerad", "selfdrive/camerad", ["./camerad"], unkillable=True, callback=driverview), - NativeProcess("clocksd", "system/clocksd", ["./clocksd"]), NativeProcess("dmonitoringmodeld", "selfdrive/modeld", ["./dmonitoringmodeld"], enabled=(not PC or WEBCAM), callback=driverview), - NativeProcess("logcatd", "system/logcatd", ["./logcatd"]), NativeProcess("encoderd", "selfdrive/loggerd", ["./encoderd"]), NativeProcess("loggerd", "selfdrive/loggerd", ["./loggerd"], onroad=False, callback=logging), NativeProcess("modeld", "selfdrive/modeld", ["./modeld"]), - NativeProcess("proclogd", "system/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 not PC else None)), @@ -38,14 +41,12 @@ procs = [ PythonProcess("controlsd", "selfdrive.controls.controlsd"), 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"), PythonProcess("radard", "selfdrive.controls.radard"), PythonProcess("thermald", "selfdrive.thermald.thermald", 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/navd/navd.py b/selfdrive/navd/navd.py index 52db9b1d08..fcd53d8b60 100755 --- a/selfdrive/navd/navd.py +++ b/selfdrive/navd/navd.py @@ -14,7 +14,7 @@ from selfdrive.navd.helpers import (Coordinate, coordinate_from_param, distance_along_geometry, maxspeed_to_ms, minimum_distance, parse_banner_instructions) -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog REROUTE_DISTANCE = 25 MANEUVER_TRANSITION_THRESHOLD = 10 diff --git a/selfdrive/sensord/rawgps/rawgpsd.py b/selfdrive/sensord/rawgps/rawgpsd.py index e0861c0d9a..3aa6d4b072 100755 --- a/selfdrive/sensord/rawgps/rawgpsd.py +++ b/selfdrive/sensord/rawgps/rawgpsd.py @@ -10,7 +10,7 @@ from struct import unpack_from, calcsize, pack import cereal.messaging as messaging from cereal import log -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog from selfdrive.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv from selfdrive.sensord.rawgps.structs import dict_unpacker diff --git a/selfdrive/sentry.py b/selfdrive/sentry.py index 10e43ee03c..aa409ea394 100644 --- a/selfdrive/sentry.py +++ b/selfdrive/sentry.py @@ -6,8 +6,8 @@ from sentry_sdk.integrations.threading import ThreadingIntegration from common.params import Params from selfdrive.athena.registration import is_registered_device from system.hardware import HARDWARE, PC -from selfdrive.swaglog import cloudlog -from selfdrive.version import get_branch, get_commit, get_origin, get_version, \ +from system.swaglog import cloudlog +from system.version import get_branch, get_commit, get_origin, get_version, \ is_comma_remote, is_dirty, is_tested_branch diff --git a/selfdrive/statsd.py b/selfdrive/statsd.py index a48ba56003..7dc002727e 100755 --- a/selfdrive/statsd.py +++ b/selfdrive/statsd.py @@ -9,10 +9,10 @@ from typing import NoReturn, Union, List, Dict from common.params import Params from cereal.messaging import SubMaster -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog from system.hardware import HARDWARE from common.file_helpers import atomic_write_in_dir -from selfdrive.version import get_normalized_origin, get_short_branch, get_short_version, is_dirty +from system.version import get_normalized_origin, get_short_branch, get_short_version, is_dirty from selfdrive.loggerd.config import STATS_DIR, STATS_DIR_FILE_LIMIT, STATS_SOCKET, STATS_FLUSH_TIME_S diff --git a/selfdrive/test/helpers.py b/selfdrive/test/helpers.py index cc8efd6e46..8cc996c28d 100644 --- a/selfdrive/test/helpers.py +++ b/selfdrive/test/helpers.py @@ -4,7 +4,7 @@ from functools import wraps from system.hardware import PC from selfdrive.manager.process_config import managed_processes -from selfdrive.version import training_version, terms_version +from system.version import training_version, terms_version def set_params_enabled(): diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py index b466aa8193..f3bb286635 100755 --- a/selfdrive/test/process_replay/model_replay.py +++ b/selfdrive/test/process_replay/model_replay.py @@ -16,7 +16,7 @@ from selfdrive.manager.process_config import managed_processes from selfdrive.test.openpilotci import BASE_URL, get_url from selfdrive.test.process_replay.compare_logs import compare_logs, save_log from selfdrive.test.process_replay.test_processes import format_diff -from selfdrive.version import get_commit +from system.version import get_commit from tools.lib.framereader import FrameReader from tools.lib.logreader import LogReader diff --git a/selfdrive/test/process_replay/test_debayer.py b/selfdrive/test/process_replay/test_debayer.py index cf939e3672..eff77fc479 100755 --- a/selfdrive/test/process_replay/test_debayer.py +++ b/selfdrive/test/process_replay/test_debayer.py @@ -9,7 +9,7 @@ import pyopencl as cl # install with `PYOPENCL_CL_PRETEND_VERSION=2.0 pip insta from system.hardware import PC, TICI from common.basedir import BASEDIR from selfdrive.test.openpilotci import BASE_URL, get_url -from selfdrive.version import get_commit +from system.version import get_commit from selfdrive.camerad.snapshot.snapshot import yuv_to_rgb from tools.lib.logreader import LogReader from tools.lib.filereader import FileReader diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index c1b5b85391..9f83a08e23 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -11,7 +11,7 @@ from selfdrive.car.car_helpers import interface_names 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 system.version import get_commit from tools.lib.filereader import FileReader from tools.lib.logreader import LogReader diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 6cf53a046e..79c3a2ebc0 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -39,7 +39,7 @@ PROCS = { "./_soundd": 1.0, "selfdrive.monitoring.dmonitoringd": 1.90, "./proclogd": 1.54, - "selfdrive.logmessaged": 0.2, + "system.logmessaged": 0.2, "./clocksd": 0.02, "./ubloxd": 0.02, "selfdrive.tombstoned": 0, diff --git a/selfdrive/thermald/fan_controller.py b/selfdrive/thermald/fan_controller.py index b1c7013297..2094faeaa7 100644 --- a/selfdrive/thermald/fan_controller.py +++ b/selfdrive/thermald/fan_controller.py @@ -3,7 +3,7 @@ from abc import ABC, abstractmethod from common.realtime import DT_TRML from common.numpy_fast import interp -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog from selfdrive.controls.lib.pid import PIDController class BaseFanController(ABC): diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py index acc2fd90bc..9f009d3265 100644 --- a/selfdrive/thermald/power_monitoring.py +++ b/selfdrive/thermald/power_monitoring.py @@ -5,7 +5,7 @@ from cereal import log from common.params import Params, put_nonblocking from common.realtime import sec_since_boot from system.hardware import HARDWARE -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog from selfdrive.statsd import statlog CAR_VOLTAGE_LOW_PASS_K = 0.091 # LPF gain for 5s tau (dt/tau / (dt/tau + 1)) diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 84663e8eb0..403e9cb232 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -20,10 +20,10 @@ from selfdrive.controls.lib.alertmanager import set_offroad_alert from system.hardware import HARDWARE, TICI, AGNOS from selfdrive.loggerd.config import get_available_percent from selfdrive.statsd import statlog -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog from selfdrive.thermald.power_monitoring import PowerMonitoring from selfdrive.thermald.fan_controller import TiciFanController -from selfdrive.version import terms_version, training_version +from system.version import terms_version, training_version ThermalStatus = log.DeviceState.ThermalStatus NetworkType = log.DeviceState.NetworkType diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index 1d0f8532db..0045e0766c 100755 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -12,8 +12,8 @@ from typing import NoReturn from common.file_helpers import mkdirs_exists_ok from selfdrive.loggerd.config import ROOT import selfdrive.sentry as sentry -from selfdrive.swaglog import cloudlog -from selfdrive.version import get_commit +from system.swaglog import cloudlog +from system.version import get_commit MAX_SIZE = 1_000_000 * 100 # allow up to 100M MAX_TOMBSTONE_FN_LEN = 62 # 85 - 23 ("/crash/") diff --git a/selfdrive/updated.py b/selfdrive/updated.py index 33e2d5d418..bdec383f52 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -38,9 +38,9 @@ from markdown_it import MarkdownIt from common.basedir import BASEDIR from common.params import Params from system.hardware import AGNOS, HARDWARE -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog from selfdrive.controls.lib.alertmanager import set_offroad_alert -from selfdrive.version import is_tested_branch +from system.version import is_tested_branch LOCK_FILE = os.getenv("UPDATER_LOCK_FILE", "/tmp/safe_staging_overlay.lock") STAGING_ROOT = os.getenv("UPDATER_STAGING_ROOT", "/data/safe_staging") diff --git a/selfdrive/logmessaged.py b/system/logmessaged.py similarity index 95% rename from selfdrive/logmessaged.py rename to system/logmessaged.py index 1d1fab5162..280a23cf1d 100755 --- a/selfdrive/logmessaged.py +++ b/system/logmessaged.py @@ -4,7 +4,7 @@ from typing import NoReturn import cereal.messaging as messaging from common.logging_extra import SwagLogFileFormatter -from selfdrive.swaglog import get_file_handler +from system.swaglog import get_file_handler def main() -> NoReturn: diff --git a/selfdrive/swaglog.py b/system/swaglog.py similarity index 100% rename from selfdrive/swaglog.py rename to system/swaglog.py diff --git a/selfdrive/timezoned.py b/system/timezoned.py similarity index 98% rename from selfdrive/timezoned.py rename to system/timezoned.py index c1d5676db6..884a5c3812 100755 --- a/selfdrive/timezoned.py +++ b/system/timezoned.py @@ -10,7 +10,7 @@ from timezonefinder import TimezoneFinder from common.params import Params from system.hardware import AGNOS -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog def set_timezone(valid_timezones, timezone): diff --git a/selfdrive/version.py b/system/version.py similarity index 99% rename from selfdrive/version.py rename to system/version.py index 08ab097344..f0817b3a9f 100644 --- a/selfdrive/version.py +++ b/system/version.py @@ -5,7 +5,7 @@ from typing import List, Optional from functools import lru_cache from common.basedir import BASEDIR -from selfdrive.swaglog import cloudlog +from system.swaglog import cloudlog TESTED_BRANCHES = ['devel', 'release3-staging', 'dashcam3-staging', 'release3', 'dashcam3'] From 1dd52ba27b6ec2121c5054c80f1820aeade40cb2 Mon Sep 17 00:00:00 2001 From: Andrew Date: Sat, 11 Jun 2022 23:38:32 -0700 Subject: [PATCH 51/93] acados build script improvements for mac (#24634) * add Darwin build w/ universal2 libs * add rust for acados rebuilds * just build script fixes Co-authored-by: Adeeb Shihadeh --- third_party/acados/build.sh | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/third_party/acados/build.sh b/third_party/acados/build.sh index 0b14a4cec9..a4246fbda6 100755 --- a/third_party/acados/build.sh +++ b/third_party/acados/build.sh @@ -1,4 +1,5 @@ -#!/usr/bin/bash -e +#!/usr/bin/env bash +set -e DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" @@ -9,6 +10,13 @@ if [ -f /TICI ]; then BLAS_TARGET="ARMV8A_ARM_CORTEX_A57" fi +ACADOS_FLAGS="-DACADOS_WITH_QPOASES=ON -UBLASFEO_TARGET -DBLASFEO_TARGET=$BLAS_TARGET" + +if [[ "$OSTYPE" == "darwin"* ]]; then + ACADOS_FLAGS="$ACADOS_FLAGS -DCMAKE_OSX_ARCHITECTURES=arm64;x86_64" + ARCHNAME="Darwin" +fi + if [ ! -d acados_repo/ ]; then git clone https://github.com/acados/acados.git $DIR/acados_repo # git clone https://github.com/commaai/acados.git $DIR/acados_repo @@ -21,7 +29,7 @@ git submodule update --recursive --init # build mkdir -p build cd build -cmake -DACADOS_WITH_QPOASES=ON -UBLASFEO_TARGET -DBLASFEO_TARGET=$BLAS_TARGET .. +cmake $ACADOS_FLAGS .. make -j20 install INSTALL_DIR="$DIR/$ARCHNAME" From 3f60088f432f0bf8e7805cdb2223817c249ec7fa Mon Sep 17 00:00:00 2001 From: Cameron Clough Date: Sun, 12 Jun 2022 17:27:46 +0100 Subject: [PATCH 52/93] Ford: disable radar for now (#24832) The newer Ford vehicles require a different radar parser. --- selfdrive/car/ford/radar_interface.py | 6 ++++++ selfdrive/car/ford/values.py | 4 ++-- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py index 1dfc27e672..866602cf09 100644 --- a/selfdrive/car/ford/radar_interface.py +++ b/selfdrive/car/ford/radar_interface.py @@ -9,6 +9,9 @@ RADAR_MSGS = list(range(0x500, 0x540)) def _create_radar_can_parser(car_fingerprint): + if DBC[car_fingerprint]['radar'] is None: + return None + msg_n = len(RADAR_MSGS) signals = list(zip(['X_Rel'] * msg_n + ['Angle'] * msg_n + ['V_Rel'] * msg_n, RADAR_MSGS * 3)) @@ -27,6 +30,9 @@ class RadarInterface(RadarInterfaceBase): self.updated_messages = set() def update(self, can_strings): + if self.rcp is None: + return super().update(None) + vls = self.rcp.update_strings(can_strings) self.updated_messages.update(vls) diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index 96ec87d757..aae011ad3d 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -79,6 +79,6 @@ FW_VERSIONS = { DBC = { - CAR.ESCAPE_MK4: dbc_dict('ford_lincoln_base_pt', 'ford_fusion_2018_adas'), - CAR.FOCUS_MK4: dbc_dict('ford_lincoln_base_pt', 'ford_fusion_2018_adas'), + CAR.ESCAPE_MK4: dbc_dict('ford_lincoln_base_pt', None), + CAR.FOCUS_MK4: dbc_dict('ford_lincoln_base_pt', None), } From 1c29b20e72141c2beae6e32cf87dabded3095fda Mon Sep 17 00:00:00 2001 From: Jeroen <33349469+jeroenlammersma@users.noreply.github.com> Date: Sun, 12 Jun 2022 18:32:18 +0200 Subject: [PATCH 53/93] Updated CARLA to v0.9.13 (#24575) * Updated CARLA to v0.9.13 * pipenv lock Co-authored-by: Adeeb Shihadeh --- Pipfile | 2 +- Pipfile.lock | 24 ++++++++++++------------ tools/sim/start_carla.sh | 4 ++-- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/Pipfile b/Pipfile index 3958c890fe..2ffe60e0fe 100644 --- a/Pipfile +++ b/Pipfile @@ -39,7 +39,7 @@ breathe = "*" subprocess32 = "*" tenacity = "*" mpld3 = "*" -carla = {version = "==0.9.12", markers="platform_system != 'Darwin'"} +carla = {version = "==0.9.13", markers="platform_system != 'Darwin'"} [packages] atomicwrites = "*" diff --git a/Pipfile.lock b/Pipfile.lock index df2b3bb853..25a97890d3 100644 --- a/Pipfile.lock +++ b/Pipfile.lock @@ -1,7 +1,7 @@ { "_meta": { "hash": { - "sha256": "4dbfaf9d7a532e0e9a126f828512a5383a817722a1580ef64b99e2427f366a72" + "sha256": "eb1eeeaabf1266fab353532d10e4d9ae36306a7577f248513909c2d6096ab1c9" }, "pipfile-spec": 6, "requires": { @@ -1155,11 +1155,11 @@ }, "setuptools": { "hashes": [ - "sha256:d1746e7fd520e83bbe210d02fff1aa1a425ad671c7a9da7d246ec2401a087198", - "sha256:e7d11f3db616cda0751372244c2ba798e8e56a28e096ec4529010b803485f3fe" + "sha256:1f5d3a1502812025cdb2e5609b6af2d207332e3f50febe6db10ed3a59b2f155f", + "sha256:30b6b0fbacc459c90d27a63e6173facfc8b8c99a48fb24b5044f459ba63cd6cf" ], "markers": "python_version >= '3.7'", - "version": "==62.3.3" + "version": "==62.3.4" }, "six": { "hashes": [ @@ -1452,17 +1452,17 @@ }, "carla": { "hashes": [ - "sha256:1ed11b56c781cd15cbd540cacfb59ad348e0a021d898cfd0ff89a585f144da0b", - "sha256:20c1e1b72034175824d89b2d86b09ae72b4aca09ea25874dc6251f239297251d", - "sha256:6d1122c24af4f6375dc6858fbb0309b61c219b101d8c5a540def4c36c4563fe1", - "sha256:9c19ebf6cbbc535bde4baf9e18c72ab349657b34c4202b9751541e4c0d20b3cc", - "sha256:a69f6d84b59e2f805b2a417de98f977fe9efe0cfa733da8d75e20d28892da915", - "sha256:c3ae0dce3f1354b6311fee21a365947b0ff169249993a913904f676046d2d69f", - "sha256:dd392a267e14b785a8f65dafef86e05a92201253e9fb4a01e1e262834f20bed2" + "sha256:1210cce213e968a644effd4e2e48458a072481459d073424b05725056ba3d77d", + "sha256:339fcb1e392f3ade1be82b7258de19c533e2efae111e954a6eb174efb296903d", + "sha256:5f065825ce812343bf27a80a19d647b3200b31b44a9e80cea0340e3bd20cdf81", + "sha256:954ca34d5bdd4516ceca353db907fee8cec6630d6b31a732b17dd1554e0f0f94", + "sha256:a64ee78fe91137fa7d4828c7fc06d5824bd7312e29e4ea4f31a5d74dd28bff40", + "sha256:a95d2d4218ea388c863c66b7c2ab3fe49ffefe53999305cfcb6a8107042f79af", + "sha256:d2bfaea2d6824a2d758cbe813856c69420494f5c97d2a2dfb45653ccf976f1ce" ], "index": "pypi", "markers": "platform_system != 'Darwin'", - "version": "==0.9.12" + "version": "==0.9.13" }, "certifi": { "hashes": [ diff --git a/tools/sim/start_carla.sh b/tools/sim/start_carla.sh index abdaee4ea8..67ced7eb21 100755 --- a/tools/sim/start_carla.sh +++ b/tools/sim/start_carla.sh @@ -15,7 +15,7 @@ if ! $(apt list --installed | grep -q nvidia-container-toolkit); then fi fi -docker pull carlasim/carla:0.9.12 +docker pull carlasim/carla:0.9.13 EXTRA_ARGS="-it" if [[ "$DETACH" ]]; then @@ -29,5 +29,5 @@ docker run \ --net=host \ -v /tmp/.X11-unix:/tmp/.X11-unix:rw \ $EXTRA_ARGS \ - carlasim/carla:0.9.12 \ + carlasim/carla:0.9.13 \ /bin/bash ./CarlaUE4.sh -opengl -nosound -RenderOffScreen -benchmark -fps=20 -quality-level=Low From 39da6912ea0b8db16d3f96f289f723452e540ab7 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sun, 12 Jun 2022 18:00:00 -0700 Subject: [PATCH 54/93] misc jenkins fixups (#24840) * bump cereal * remove that * pull cl image * lil docker cleanup --- Dockerfile.openpilot | 7 +------ cereal | 2 +- selfdrive/manager/test/test_manager.py | 8 ++------ tools/sim/Dockerfile.sim | 10 +++++----- tools/sim/build_container.sh | 2 +- 5 files changed, 10 insertions(+), 19 deletions(-) diff --git a/Dockerfile.openpilot b/Dockerfile.openpilot index 66b9f18a11..102da78d7d 100644 --- a/Dockerfile.openpilot +++ b/Dockerfile.openpilot @@ -8,11 +8,6 @@ ENV PYTHONPATH ${OPENPILOT_PATH}:${PYTHONPATH} RUN mkdir -p ${OPENPILOT_PATH} WORKDIR ${OPENPILOT_PATH} -COPY Pipfile Pipfile.lock $OPENPILOT_PATH -RUN pip install --no-cache-dir pipenv==2021.5.29 pip==21.3.1 && \ - pipenv install --system --deploy --dev --clear && \ - pip uninstall -y pipenv - COPY SConstruct ${OPENPILOT_PATH} COPY ./pyextra ${OPENPILOT_PATH}/pyextra @@ -30,4 +25,4 @@ COPY ./panda ${OPENPILOT_PATH}/panda COPY ./selfdrive ${OPENPILOT_PATH}/selfdrive COPY ./system ${OPENPILOT_PATH}/system -RUN scons -j$(nproc) +RUN scons --cache-readonly -j$(nproc) diff --git a/cereal b/cereal index 78870ba056..98f795fd73 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 78870ba056174352218988610e5010fde4eca956 +Subproject commit 98f795fd73356198f1fc8d5ea5a8f25e6f7c57e0 diff --git a/selfdrive/manager/test/test_manager.py b/selfdrive/manager/test/test_manager.py index e4c3ef7c4c..a84ff264d2 100755 --- a/selfdrive/manager/test/test_manager.py +++ b/selfdrive/manager/test/test_manager.py @@ -5,14 +5,13 @@ import time import unittest import selfdrive.manager.manager as manager -from system.hardware import AGNOS, HARDWARE from selfdrive.manager.process import DaemonProcess from selfdrive.manager.process_config import managed_processes +from system.hardware import AGNOS, HARDWARE os.environ['FAKEUPLOAD'] = "1" -# TODO: make eon fast -MAX_STARTUP_TIME = 15 +MAX_STARTUP_TIME = 3 ALL_PROCESSES = [p.name for p in managed_processes.values() if (type(p) is not DaemonProcess) and p.enabled and (p.name not in ['updated', 'pandad'])] @@ -54,9 +53,6 @@ class TestManager(unittest.TestCase): # TODO: make Qt UI exit gracefully continue - # Make sure the process is actually dead - managed_processes[p].stop() - # TODO: interrupted blocking read exits with 1 in cereal. use a more unique return code exit_codes = [0, 1] if managed_processes[p].sigkill: diff --git a/tools/sim/Dockerfile.sim b/tools/sim/Dockerfile.sim index d838efed13..0d6e8e584c 100644 --- a/tools/sim/Dockerfile.sim +++ b/tools/sim/Dockerfile.sim @@ -1,9 +1,9 @@ FROM ghcr.io/commaai/openpilot-base-cl:latest -RUN apt-get update && apt-get install -y --no-install-recommends\ - tmux \ - vim \ - && rm -rf /var/lib/apt/lists/* +RUN apt-get update && apt-get install -y --no-install-recommends \ + tmux \ + vim \ + && rm -rf /var/lib/apt/lists/* # get same tmux config used on NEOS for debugging RUN cd $HOME && \ @@ -27,6 +27,6 @@ COPY ./system $HOME/openpilot/system COPY ./tools $HOME/openpilot/tools WORKDIR $HOME/openpilot -RUN scons -j12 +RUN scons --cache-readonly -j12 RUN python -c "from selfdrive.test.helpers import set_params_enabled; set_params_enabled()" diff --git a/tools/sim/build_container.sh b/tools/sim/build_container.sh index 451277d590..81afb82d83 100755 --- a/tools/sim/build_container.sh +++ b/tools/sim/build_container.sh @@ -3,7 +3,7 @@ DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" cd $DIR/../../ -docker pull ghcr.io/commaai/openpilot-base:latest +docker pull ghcr.io/commaai/openpilot-base-cl:latest docker build \ --cache-from ghcr.io/commaai/openpilot-sim:latest \ -t ghcr.io/commaai/openpilot-sim:latest \ From e7cad559a659d47466645290e00ee6205bb938c6 Mon Sep 17 00:00:00 2001 From: Cameron Clough Date: Mon, 13 Jun 2022 05:39:36 +0100 Subject: [PATCH 55/93] Ford: remove Fusion DBC from release (#24841) --- release/files_common | 1 - 1 file changed, 1 deletion(-) diff --git a/release/files_common b/release/files_common index 76d440c402..6731ae4a12 100644 --- a/release/files_common +++ b/release/files_common @@ -473,7 +473,6 @@ opendbc/gm_global_a_powertrain_generated.dbc opendbc/gm_global_a_object.dbc opendbc/gm_global_a_chassis.dbc -opendbc/ford_fusion_2018_adas.dbc opendbc/ford_lincoln_base_pt.dbc opendbc/honda_accord_2018_can_generated.dbc From 3db36a1958b24023a8af2a10419c0c7e4112bef7 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sun, 12 Jun 2022 21:59:58 -0700 Subject: [PATCH 56/93] jenkins: lock simulator --- Jenkinsfile | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/Jenkinsfile b/Jenkinsfile index 9d1ab33761..ebc26a5920 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -89,8 +89,10 @@ pipeline { sh "git config --global --add safe.directory ${WORKSPACE}" sh "git lfs pull" sh "${WORKSPACE}/tools/sim/build_container.sh" - sh "DETACH=1 ${WORKSPACE}/tools/sim/start_carla.sh" - sh "${WORKSPACE}/tools/sim/start_openpilot_docker.sh" + lock(resource: "", label: "simulator", inversePrecedence: true, quantity: 1) { + sh "DETACH=1 ${WORKSPACE}/tools/sim/start_carla.sh" + sh "${WORKSPACE}/tools/sim/start_openpilot_docker.sh" + } } post { From 84c741ecf6ac0727a29aeebb5f2a6539c2d76dd2 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 13 Jun 2022 09:37:12 +0200 Subject: [PATCH 57/93] SConstruct: set AGNOS from /AGNOS --- SConstruct | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SConstruct b/SConstruct index 0aeb382c23..edf14b4725 100644 --- a/SConstruct +++ b/SConstruct @@ -6,7 +6,7 @@ import platform import numpy as np TICI = os.path.isfile('/TICI') -AGNOS = TICI +AGNOS = os.path.isfile('/AGNOS') Decider('MD5-timestamp') From d71295e0451260acdcfdfb14477c0e21bc4ed311 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 13 Jun 2022 09:50:40 +0200 Subject: [PATCH 58/93] Revert "SConstruct: set AGNOS from /AGNOS" until Jenkins devices are updated This reverts commit 84c741ecf6ac0727a29aeebb5f2a6539c2d76dd2. --- SConstruct | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SConstruct b/SConstruct index edf14b4725..0aeb382c23 100644 --- a/SConstruct +++ b/SConstruct @@ -6,7 +6,7 @@ import platform import numpy as np TICI = os.path.isfile('/TICI') -AGNOS = os.path.isfile('/AGNOS') +AGNOS = TICI Decider('MD5-timestamp') From 724b322909152d58f626b6e76fd34d7d03d67250 Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Mon, 13 Jun 2022 11:45:35 +0200 Subject: [PATCH 59/93] Laikad: Use filter for correcting measurements (#24824) * Update laikad. * Update log error --- SConstruct | 2 +- selfdrive/locationd/laikad.py | 65 ++++++++++++++++++++++------------- 2 files changed, 42 insertions(+), 25 deletions(-) diff --git a/SConstruct b/SConstruct index 0aeb382c23..b5c4edc99b 100644 --- a/SConstruct +++ b/SConstruct @@ -359,6 +359,7 @@ Export('cereal', 'messaging', 'visionipc') rednose_config = { 'generated_folder': '#selfdrive/locationd/models/generated', 'to_build': { + 'gnss': ('#selfdrive/locationd/models/gnss_kf.py', True, []), 'live': ('#selfdrive/locationd/models/live_kf.py', True, ['live_kf_constants.h']), 'car': ('#selfdrive/locationd/models/car_kf.py', True, []), }, @@ -366,7 +367,6 @@ rednose_config = { if arch != "larch64": rednose_config['to_build'].update({ - 'gnss': ('#selfdrive/locationd/models/gnss_kf.py', True, []), 'loc_4': ('#selfdrive/locationd/models/loc_kf.py', True, []), 'pos_computer_4': ('#rednose/helpers/lst_sq_computer.py', False, []), 'pos_computer_5': ('#rednose/helpers/lst_sq_computer.py', False, []), diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 01457f4681..42c4156795 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -6,6 +6,8 @@ from typing import List, Optional import numpy as np from collections import defaultdict +from numpy.linalg import linalg + from cereal import log, messaging from laika import AstroDog from laika.constants import SECS_IN_MIN @@ -37,24 +39,33 @@ class Laikad: latest_msg_t = GPSTime(report.gpsWeek, report.rcvTow) self.fetch_orbits(latest_msg_t + SECS_IN_MIN, block) new_meas = read_raw_ublox(report) + processed_measurements = process_measurements(new_meas, self.astro_dog) + pos_fix = calc_pos_fix(processed_measurements, min_measurements=4) - measurements = process_measurements(new_meas, self.astro_dog) - pos_fix = calc_pos_fix(measurements, min_measurements=4) - # To get a position fix a minimum of 5 measurements are needed. - # Each report can contain less and some measurements can't be processed. + t = ublox_mono_time * 1e-9 + kf_pos_std = None + if all(self.kf_valid(t)): + self.gnss_kf.predict(t) + kf_pos_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_POS].diagonal())) + # If localizer is valid use its position to correct measurements + if kf_pos_std is not None and linalg.norm(kf_pos_std) < 100: + est_pos = self.gnss_kf.x[GStates.ECEF_POS] + elif len(pos_fix) > 0 and abs(np.array(pos_fix[1])).mean() < 1000: + est_pos = pos_fix[0][:3] + else: + est_pos = None corrected_measurements = [] - if len(pos_fix) > 0 and abs(np.array(pos_fix[1])).mean() < 1000: - corrected_measurements = correct_measurements(measurements, pos_fix[0][:3], self.astro_dog) + if est_pos is not None: + corrected_measurements = correct_measurements(processed_measurements, est_pos, self.astro_dog) - t = ublox_mono_time * 1e-9 self.update_localizer(pos_fix, t, corrected_measurements) - localizer_valid = self.localizer_valid(t) + kf_valid = all(self.kf_valid(t)) ecef_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist() ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist() - pos_std = self.gnss_kf.P[GStates.ECEF_POS].flatten().tolist() - vel_std = self.gnss_kf.P[GStates.ECEF_VELOCITY].flatten().tolist() + pos_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_POS].diagonal())).tolist() + vel_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_VELOCITY].diagonal())).tolist() bearing_deg, bearing_std = get_bearing_from_gnss(ecef_pos, ecef_vel, vel_std) @@ -62,9 +73,9 @@ class Laikad: dat = messaging.new_message("gnssMeasurements") measurement_msg = log.LiveLocationKalman.Measurement.new_message dat.gnssMeasurements = { - "positionECEF": measurement_msg(value=ecef_pos, std=pos_std, valid=localizer_valid), - "velocityECEF": measurement_msg(value=ecef_vel, std=vel_std, valid=localizer_valid), - "bearingDeg": measurement_msg(value=[bearing_deg], std=[bearing_std], valid=localizer_valid), + "positionECEF": measurement_msg(value=ecef_pos, std=pos_std, valid=kf_valid), + "velocityECEF": measurement_msg(value=ecef_vel, std=vel_std, valid=kf_valid), + "bearingDeg": measurement_msg(value=[bearing_deg], std=[bearing_std], valid=kf_valid), "ubloxMonoTime": ublox_mono_time, "correctedMeasurements": meas_msgs } @@ -77,18 +88,21 @@ class Laikad: def update_localizer(self, pos_fix, t: float, measurements: List[GNSSMeasurement]): # Check time and outputs are valid - if not self.localizer_valid(t): - # A position fix is needed when resetting the kalman filter. - if len(pos_fix) == 0: - return - post_est = pos_fix[0][:3].tolist() - filter_time = self.gnss_kf.filter.filter_time - if filter_time is None: + valid = self.kf_valid(t) + if not all(valid): + if not valid[0]: cloudlog.info("Init gnss kalman filter") - elif abs(t - filter_time) > MAX_TIME_GAP: + elif not valid[1]: cloudlog.error("Time gap of over 10s detected, gnss kalman reset") - else: + elif not valid[2]: cloudlog.error("Gnss kalman filter state is nan") + else: + cloudlog.error("Gnss kalman std too far") + + if len(pos_fix) == 0: + cloudlog.error("Position fix not available when resetting kalman filter") + return + post_est = pos_fix[0][:3].tolist() self.init_gnss_localizer(post_est) if len(measurements) > 0: kf_add_observations(self.gnss_kf, t, measurements) @@ -96,9 +110,12 @@ class Laikad: # Ensure gnss filter is updated even with no new measurements self.gnss_kf.predict(t) - def localizer_valid(self, t: float): + def kf_valid(self, t: float): filter_time = self.gnss_kf.filter.filter_time - return filter_time is not None and (t - filter_time) < MAX_TIME_GAP and all(np.isfinite(self.gnss_kf.x[GStates.ECEF_POS])) + return [filter_time is not None, + filter_time is not None and abs(t - filter_time) < MAX_TIME_GAP, + all(np.isfinite(self.gnss_kf.x[GStates.ECEF_POS])), + linalg.norm(self.gnss_kf.P[GStates.ECEF_POS]) < 1e5] def init_gnss_localizer(self, est_pos): x_initial, p_initial_diag = np.copy(GNSSKalman.x_initial), np.copy(np.diagonal(GNSSKalman.P_initial)) From 1221aef233180a7d290ff813d2e2e53dbf87d586 Mon Sep 17 00:00:00 2001 From: Joost Wooning Date: Mon, 13 Jun 2022 12:27:47 +0200 Subject: [PATCH 60/93] ui: skip texture frame copy (#24700) * ui_blit working * simpler and working * more believable that it's real * working on device * build on pc * use hardware pc * reduce cpu usage * yuv conversion to EGL * move everything to cameraview * some cleanup * more cleanup * init array * init images with std::map * dont destroy images * do destroy images Co-authored-by: Comma Device --- selfdrive/test/test_onroad.py | 2 +- selfdrive/ui/SConscript | 3 ++ selfdrive/ui/qt/widgets/cameraview.cc | 72 ++++++++++++++++++++++++--- selfdrive/ui/qt/widgets/cameraview.h | 17 ++++++- 4 files changed, 84 insertions(+), 10 deletions(-) diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 79c3a2ebc0..3586f86f12 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -27,7 +27,7 @@ PROCS = { "./camerad": 16.5, "./locationd": 9.1, "selfdrive.controls.plannerd": 11.7, - "./_ui": 26.4, + "./_ui": 19.2, "selfdrive.locationd.paramsd": 9.0, "./_sensord": 6.17, "selfdrive.controls.radard": 4.5, diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 47c4ac2a51..28fcc5f56f 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -5,6 +5,9 @@ Import('qt_env', 'arch', 'common', 'messaging', 'visionipc', base_libs = [common, messaging, cereal, visionipc, transformations, 'zmq', 'capnp', 'kj', 'm', 'OpenCL', 'ssl', 'crypto', 'pthread'] + qt_env["LIBS"] +if arch == 'larch64': + base_libs.append('EGL') + maps = arch in ['larch64', 'x86_64'] if maps and arch == 'x86_64': diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index f16e8e4e0d..000ac48475 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -26,6 +26,18 @@ const char frame_vertex_shader[] = " vTexCoord = aTexCoord;\n" "}\n"; +#ifdef QCOM2 +const char frame_fragment_shader[] = + "#version 300 es\n" + "#extension GL_OES_EGL_image_external_essl3 : enable\n" + "precision mediump float;\n" + "uniform samplerExternalOES uTexture;\n" + "in vec2 vTexCoord;\n" + "out vec4 colorOut;\n" + "void main() {\n" + " colorOut = texture(uTexture, vTexCoord);\n" + "}\n"; +#else const char frame_fragment_shader[] = #ifdef __APPLE__ "#version 330 core\n" @@ -45,6 +57,7 @@ const char frame_fragment_shader[] = " float b = y + 1.772 * uv.x;\n" " colorOut = vec4(r, g, b, 1.0);\n" "}\n"; +#endif const mat4 device_transform = {{ 1.0, 0.0, 0.0, 0.0, @@ -99,7 +112,7 @@ CameraViewWidget::~CameraViewWidget() { glDeleteVertexArrays(1, &frame_vao); glDeleteBuffers(1, &frame_vbo); glDeleteBuffers(1, &frame_ibo); - glDeleteBuffers(3, textures); + glDeleteBuffers(2, textures); } doneCurrent(); } @@ -143,10 +156,15 @@ void CameraViewWidget::initializeGL() { glBindBuffer(GL_ARRAY_BUFFER, 0); glBindVertexArray(0); - glGenTextures(3, textures); glUseProgram(program->programId()); + +#ifdef QCOM2 + glUniform1i(program->uniformLocation("uTexture"), 0); +#else + glGenTextures(2, textures); glUniform1i(program->uniformLocation("uTextureY"), 0); glUniform1i(program->uniformLocation("uTextureUV"), 1); +#endif } void CameraViewWidget::showEvent(QShowEvent *event) { @@ -207,29 +225,33 @@ void CameraViewWidget::paintGL() { for (frame_idx = 0; frame_idx < frames.size() - 1; frame_idx++) { if (frames[frame_idx].first == draw_frame_id) break; } - 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()); + glPixelStorei(GL_UNPACK_ALIGNMENT, 1); + VisionBuf *frame = frames[frame_idx].second; + +#ifdef QCOM2 + glActiveTexture(GL_TEXTURE0); + glEGLImageTargetTexture2DOES(GL_TEXTURE_EXTERNAL_OES, egl_images[frame->idx]); + assert(glGetError() == GL_NO_ERROR); +#else + glPixelStorei(GL_UNPACK_ROW_LENGTH, stream_stride); 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); +#endif glUniformMatrix4fv(program->uniformLocation("uTransform"), 1, GL_TRUE, frame_mat.v); - assert(glGetError() == GL_NO_ERROR); glEnableVertexAttribArray(0); glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_BYTE, (const void *)0); glDisableVertexAttribArray(0); @@ -247,6 +269,32 @@ void CameraViewWidget::vipcConnected(VisionIpcClient *vipc_client) { stream_height = vipc_client->buffers[0].height; stream_stride = vipc_client->buffers[0].stride; +#ifdef QCOM2 + egl_display = eglGetCurrentDisplay(); + + for (auto &pair : egl_images) { + eglDestroyImageKHR(egl_display, pair.second); + } + egl_images.clear(); + + for (int i = 0; i < vipc_client->num_buffers; i++) { // import buffers into OpenGL + int fd = dup(vipc_client->buffers[i].fd); // eglDestroyImageKHR will close, so duplicate + EGLint img_attrs[] = { + EGL_WIDTH, (int)vipc_client->buffers[i].width, + EGL_HEIGHT, (int)vipc_client->buffers[i].height, + EGL_LINUX_DRM_FOURCC_EXT, DRM_FORMAT_NV12, + EGL_DMA_BUF_PLANE0_FD_EXT, fd, + EGL_DMA_BUF_PLANE0_OFFSET_EXT, 0, + EGL_DMA_BUF_PLANE0_PITCH_EXT, (int)vipc_client->buffers[i].stride, + EGL_DMA_BUF_PLANE1_FD_EXT, fd, + EGL_DMA_BUF_PLANE1_OFFSET_EXT, (int)vipc_client->buffers[i].uv_offset, + EGL_DMA_BUF_PLANE1_PITCH_EXT, (int)vipc_client->buffers[i].stride, + EGL_NONE + }; + egl_images[i] = eglCreateImageKHR(egl_display, EGL_NO_CONTEXT, EGL_LINUX_DMA_BUF_EXT, 0, img_attrs); + assert(eglGetError() == EGL_SUCCESS); + } +#else 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); @@ -262,6 +310,7 @@ void CameraViewWidget::vipcConnected(VisionIpcClient *vipc_client) { 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); +#endif updateFrameMat(width(), height()); } @@ -297,4 +346,11 @@ void CameraViewWidget::vipcThread() { emit vipcThreadFrameReceived(buf, meta_main.frame_id); } } + +#ifdef QCOM2 + for (auto &pair : egl_images) { + eglDestroyImageKHR(egl_display, pair.second); + } + egl_images.clear(); +#endif } diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h index 953cbed00b..ddc3fc253b 100644 --- a/selfdrive/ui/qt/widgets/cameraview.h +++ b/selfdrive/ui/qt/widgets/cameraview.h @@ -6,6 +6,16 @@ #include #include #include + +#ifdef QCOM2 +#define EGL_EGLEXT_PROTOTYPES +#define EGL_NO_X11 +#define GL_TEXTURE_EXTERNAL_OES 0x8D65 +#include +#include +#include +#endif + #include "cereal/visionipc/visionipc_client.h" #include "selfdrive/camerad/cameras/camera_common.h" #include "selfdrive/ui/ui.h" @@ -41,11 +51,16 @@ protected: bool zoomed_view; GLuint frame_vao, frame_vbo, frame_ibo; - GLuint textures[3]; + GLuint textures[2]; mat4 frame_mat; std::unique_ptr program; QColor bg = QColor("#000000"); +#ifdef QCOM2 + EGLDisplay egl_display; + std::map egl_images; +#endif + std::string stream_name; int stream_width = 0; int stream_height = 0; From a2d2378ee147f2db59aecd3401677cb2427d1ced Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Mon, 13 Jun 2022 14:02:31 +0200 Subject: [PATCH 61/93] Laikad: process executor to fetch orbits (#24843) * Use ProcessPoolExecutor to fetch orbits * update laika repo * Minor --- laika_repo | 2 +- selfdrive/locationd/laikad.py | 64 ++++++++++++------------- selfdrive/locationd/test/test_laikad.py | 33 +++++++++---- 3 files changed, 57 insertions(+), 42 deletions(-) diff --git a/laika_repo b/laika_repo index d871946134..36f2621fc5 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit d87194613455b42af19ff2b5a3f7d1cae5852885 +Subproject commit 36f2621fc5348487bb2cd606c37c8c15de0e32cd diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 42c4156795..33e41398a0 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 import time -from multiprocessing import Process, Queue +from concurrent.futures import Future, ProcessPoolExecutor from typing import List, Optional import numpy as np @@ -10,7 +10,7 @@ from numpy.linalg import linalg from cereal import log, messaging from laika import AstroDog -from laika.constants import SECS_IN_MIN +from laika.constants import SECS_IN_HR, SECS_IN_MIN from laika.ephemeris import EphemerisType, convert_ublox_ephem from laika.gps_time import GPSTime from laika.helpers import ConstellationId @@ -29,8 +29,9 @@ class Laikad: def __init__(self, valid_const=("GPS", "GLONASS"), auto_update=False, valid_ephem_types=(EphemerisType.ULTRA_RAPID_ORBIT, EphemerisType.NAV)): self.astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types) self.gnss_kf = GNSSKalman(GENERATED_DIR) - self.orbit_p: Optional[Process] = None - self.orbit_q = Queue() + self.orbit_fetch_executor = ProcessPoolExecutor() + self.orbit_fetch_future: Optional[Future] = None + self.last_fetch_orbits_t = None def process_ublox_msg(self, ublox_msg, ublox_mono_time: int, block=False): if ublox_msg.which == 'measurementReport': @@ -82,7 +83,7 @@ class Laikad: return dat elif ublox_msg.which == 'ephemeris': ephem = convert_ublox_ephem(ublox_msg.ephemeris) - self.astro_dog.add_ephems([ephem], self.astro_dog.nav) + self.astro_dog.add_navs([ephem]) # elif ublox_msg.which == 'ionoData': # todo add this. Needed to better correct messages offline. First fix ublox_msg.cc to sent them. @@ -100,7 +101,7 @@ class Laikad: cloudlog.error("Gnss kalman std too far") if len(pos_fix) == 0: - cloudlog.error("Position fix not available when resetting kalman filter") + cloudlog.warning("Position fix not available when resetting kalman filter") return post_est = pos_fix[0][:3].tolist() self.init_gnss_localizer(post_est) @@ -124,36 +125,33 @@ class Laikad: self.gnss_kf.init_state(x_initial, covs_diag=p_initial_diag) - def get_orbit_data(self, t: GPSTime, queue): - cloudlog.info(f"Start to download/parse orbits for time {t.as_datetime()}") - start_time = time.monotonic() - try: - self.astro_dog.get_orbit_data(t, only_predictions=True) - except RuntimeError as e: - cloudlog.info(f"No orbit data found. {e}") - return - cloudlog.info(f"Done parsing orbits. Took {time.monotonic() - start_time:.2f}s") - if queue is not None: - queue.put((self.astro_dog.orbits, self.astro_dog.orbit_fetched_times)) - def fetch_orbits(self, t: GPSTime, block): - if t not in self.astro_dog.orbit_fetched_times: - if block: - self.get_orbit_data(t, None) - return - if self.orbit_p is None: - self.orbit_p = Process(target=self.get_orbit_data, args=(t, self.orbit_q)) - self.orbit_p.start() - if not self.orbit_q.empty(): - ret = self.orbit_q.get() + if t not in self.astro_dog.orbit_fetched_times and (self.last_fetch_orbits_t is None or t - self.last_fetch_orbits_t > SECS_IN_HR): + astro_dog_vars = self.astro_dog.valid_const, self.astro_dog.auto_update, self.astro_dog.valid_ephem_types + if self.orbit_fetch_future is None: + self.orbit_fetch_future = self.orbit_fetch_executor.submit(get_orbit_data, t, *astro_dog_vars) + if block: + self.orbit_fetch_future.result() + if self.orbit_fetch_future.done(): + ret = self.orbit_fetch_future.result() if ret: self.astro_dog.orbits, self.astro_dog.orbit_fetched_times = ret - self.orbit_p.join() - self.orbit_p = None - - def __del__(self): - if self.orbit_p is not None: - self.orbit_p.kill() + self.orbit_fetch_future = None + self.last_fetch_orbits_t = t + + +def get_orbit_data(t: GPSTime, valid_const, auto_update, valid_ephem_types): + astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types) + cloudlog.info(f"Start to download/parse orbits for time {t.as_datetime()}") + start_time = time.monotonic() + data = None + try: + astro_dog.get_orbit_data(t, only_predictions=True) + data = (astro_dog.orbits, astro_dog.orbit_fetched_times) + except RuntimeError as e: + cloudlog.info(f"No orbit data found. {e}") + cloudlog.info(f"Done parsing orbits. Took {time.monotonic() - start_time:.1f}s") + return data def create_measurement_msg(meas: GNSSMeasurement): diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py index 7a8c1ecb13..7dc803d799 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/selfdrive/locationd/test/test_laikad.py @@ -69,29 +69,46 @@ class TestLaikad(unittest.TestCase): self.assertEqual(256, len(correct_msgs)) self.assertEqual(256, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) - def test_laika_get_orbits(self): - laikad = Laikad(auto_update=False) - first_gps_time = None + def get_first_gps_time(self): for m in self.logs: if m.ubloxGnss.which == 'measurementReport': new_meas = read_raw_ublox(m.ubloxGnss.measurementReport) if len(new_meas) != 0: - first_gps_time = new_meas[0].recv_time - break + return new_meas[0].recv_time + + def test_laika_get_orbits(self): + laikad = Laikad(auto_update=False) + first_gps_time = self.get_first_gps_time() # Pretend process has loaded the orbits on startup by using the time of the first gps message. laikad.fetch_orbits(first_gps_time, block=True) - self.assertEqual(29, len(laikad.astro_dog.orbits.keys())) + self.assertEqual(29, len(laikad.astro_dog.orbits.values())) + self.assertGreater(min([len(v) for v in laikad.astro_dog.orbits.values()]), 0) @unittest.skip("Use to debug live data") def test_laika_get_orbits_now(self): laikad = Laikad(auto_update=False) laikad.fetch_orbits(GPSTime.from_datetime(datetime.utcnow()), block=True) prn = "G01" - self.assertLess(0, len(laikad.astro_dog.orbits[prn])) + self.assertGreater(len(laikad.astro_dog.orbits[prn]), 0) prn = "R01" - self.assertLess(0, len(laikad.astro_dog.orbits[prn])) + self.assertGreater(len(laikad.astro_dog.orbits[prn]), 0) print(min(laikad.astro_dog.orbits[prn], key=lambda e: e.epoch).epoch.as_datetime()) + def test_get_orbits_in_process(self): + laikad = Laikad(auto_update=False) + has_orbits = False + for m in self.logs: + laikad.process_ublox_msg(m.ubloxGnss, m.logMonoTime, block=False) + if laikad.orbit_fetch_future is not None: + laikad.orbit_fetch_future.result() + vals = laikad.astro_dog.orbits.values() + has_orbits = len(vals) > 0 and max([len(v) for v in vals]) > 0 + if has_orbits: + break + self.assertTrue(has_orbits) + self.assertGreater(len(laikad.astro_dog.orbit_fetched_times._ranges), 0) + self.assertEqual(None, laikad.orbit_fetch_future) + if __name__ == "__main__": unittest.main() From fb068f04f50fcde6906072b4963114146852c51d Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 13 Jun 2022 16:43:50 +0200 Subject: [PATCH 62/93] camerad: remove unused SubMaster (#24844) --- selfdrive/camerad/cameras/camera_qcom2.cc | 7 ++----- selfdrive/camerad/cameras/camera_qcom2.h | 1 - selfdrive/camerad/test/camera_test.h | 1 - 3 files changed, 2 insertions(+), 7 deletions(-) diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc index d43beb0921..c1ffc1275a 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.cc +++ b/selfdrive/camerad/cameras/camera_qcom2.cc @@ -837,7 +837,6 @@ void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_i s->road_cam.camera_init(s, v, CAMERA_ID_AR0231, 1, 20, device_id, ctx, VISION_STREAM_RGB_ROAD, VISION_STREAM_ROAD, !env_disable_road); s->wide_road_cam.camera_init(s, v, CAMERA_ID_AR0231, 0, 20, device_id, ctx, VISION_STREAM_RGB_WIDE_ROAD, VISION_STREAM_WIDE_ROAD, !env_disable_wide_road); - s->sm = new SubMaster({"driverState"}); s->pm = new PubMaster({"roadCameraState", "driverCameraState", "wideRoadCameraState", "thumbnail"}); } @@ -948,7 +947,6 @@ void cameras_close(MultiCameraState *s) { s->road_cam.camera_close(); s->wide_road_cam.camera_close(); - delete s->sm; delete s->pm; } @@ -1221,7 +1219,7 @@ static void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal framed.setTemperaturesC({temp_0, temp_1}); } -static void driver_cam_auto_exposure(CameraState *c, SubMaster &sm) { +static void driver_cam_auto_exposure(CameraState *c) { struct ExpRect {int x1, x2, x_skip, y1, y2, y_skip;}; const CameraBuf *b = &c->buf; static ExpRect rect = {96, 1832, 2, 242, 1148, 4}; @@ -1229,8 +1227,7 @@ static void driver_cam_auto_exposure(CameraState *c, SubMaster &sm) { } static void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) { - s->sm->update(0); - driver_cam_auto_exposure(c, *(s->sm)); + driver_cam_auto_exposure(c); MessageBuilder msg; auto framed = msg.initEvent().initDriverCameraState(); diff --git a/selfdrive/camerad/cameras/camera_qcom2.h b/selfdrive/camerad/cameras/camera_qcom2.h index d869620e9a..88766a68e9 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.h +++ b/selfdrive/camerad/cameras/camera_qcom2.h @@ -106,6 +106,5 @@ typedef struct MultiCameraState { CameraState wide_road_cam; CameraState driver_cam; - SubMaster *sm; PubMaster *pm; } MultiCameraState; diff --git a/selfdrive/camerad/test/camera_test.h b/selfdrive/camerad/test/camera_test.h index cc521db397..a9f213c923 100644 --- a/selfdrive/camerad/test/camera_test.h +++ b/selfdrive/camerad/test/camera_test.h @@ -19,7 +19,6 @@ typedef struct MultiCameraState { CameraState road_cam; CameraState driver_cam; - SubMaster *sm = nullptr; PubMaster *pm = nullptr; } MultiCameraState; From 7bca95dbb89dc6b969871cc7f6bf4306d91f60cc Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 13 Jun 2022 16:44:38 +0200 Subject: [PATCH 63/93] navd: speed limits only when localizer is valid (#24845) --- selfdrive/navd/navd.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index fcd53d8b60..509653a46d 100755 --- a/selfdrive/navd/navd.py +++ b/selfdrive/navd/navd.py @@ -32,6 +32,7 @@ class RouteEngine: self.last_bearing = None self.gps_ok = False + self.localizer_valid = False self.nav_destination = None self.step_idx = None @@ -73,9 +74,9 @@ class RouteEngine: location = self.sm['liveLocationKalman'] self.gps_ok = location.gpsOK - localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid + self.localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid - if localizer_valid: + if self.localizer_valid: self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2]) self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1]) @@ -202,7 +203,7 @@ class RouteEngine: if along_geometry < distance_along_geometry(geometry, geometry[closest_idx]): closest = geometry[closest_idx - 1] - if 'maxspeed' in closest.annotations: + if ('maxspeed' in closest.annotations) and self.localizer_valid: msg.navInstruction.speedLimit = closest.annotations['maxspeed'] # Speed limit sign type From 566de12671f7fbf299306339c0ca901754f7f3ca Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 13 Jun 2022 09:29:28 -0700 Subject: [PATCH 64/93] Toyota Camry TSS2: update torque control params (#24819) Use updated accel and friction values for TSS2 Camry --- selfdrive/car/toyota/interface.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 83eed611c3..53accecfe0 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -31,6 +31,7 @@ class CarInterface(CarInterfaceBase): ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop stop_and_go = False + torque_params = CarInterfaceBase.get_torque_params(candidate) if candidate == CAR.PRIUS: stop_and_go = True @@ -91,8 +92,7 @@ class CarInterface(CarInterfaceBase): tire_stiffness_factor = 0.7933 ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid if candidate in (CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2): - ret.maxLateralAccel = 2.4 - set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=ret.maxLateralAccel, FRICTION=0.05) + set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, torque_params['LAT_ACCEL_FACTOR'], torque_params['FRICTION']) else: set_lat_tune(ret.lateralTuning, LatTunes.PID_C) From f05166ae26eb8165f35e8205b59c36e618d179b8 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Mon, 13 Jun 2022 13:58:22 -0700 Subject: [PATCH 65/93] ui.py: update for nv12 --- tools/replay/ui.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/replay/ui.py b/tools/replay/ui.py index 1807fb7747..bbcd49061e 100755 --- a/tools/replay/ui.py +++ b/tools/replay/ui.py @@ -118,7 +118,7 @@ def ui_thread(addr): imgff = np.frombuffer(yuv_img_raw, dtype=np.uint8).reshape((vipc_client.height * 3 // 2, vipc_client.width)) num_px = vipc_client.width * vipc_client.height - bgr = cv2.cvtColor(imgff, cv2.COLOR_YUV2RGB_I420) + bgr = cv2.cvtColor(imgff, cv2.COLOR_YUV2RGB_NV12) zoom_matrix = _BB_TO_FULL_FRAME[num_px] cv2.warpAffine(bgr, zoom_matrix[:2], (img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP) From cbd404b954b5b80b65f1ad8363b0245c3c7c7911 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Mon, 13 Jun 2022 16:38:32 -0700 Subject: [PATCH 66/93] Revert "thermald: consider pmic in component temp management (#24708)" This reverts commit c8c21baf50865b0db2cc9345901bb98904cfbaaf. --- 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 403e9cb232..b909d1198c 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -249,7 +249,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(msg.deviceState.pmicTempC)) + max(max(msg.deviceState.cpuTempC), msg.deviceState.memoryTempC, max(msg.deviceState.gpuTempC)) ) if fan_controller is not None: From be13fc71f1237eb56a9d1cf2d7a0feb11b55d0b7 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Mon, 13 Jun 2022 16:54:32 -0700 Subject: [PATCH 67/93] Couple more cars to torque tune (#24848) * try sonata on torque tune * Couple known cars to torque control * fix * more fix --- selfdrive/car/hyundai/interface.py | 22 ++++++++-------------- 1 file changed, 8 insertions(+), 14 deletions(-) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 08af654996..6fd75ebfab 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -55,7 +55,7 @@ class CarInterface(CarInterfaceBase): ret.stopAccel = 0.0 ret.longitudinalActuatorDelayUpperBound = 1.0 # s - + torque_params = CarInterfaceBase.get_torque_params(candidate) if candidate in (CAR.SANTA_FE, CAR.SANTA_FE_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022): ret.lateralTuning.pid.kf = 0.00005 ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG @@ -66,13 +66,11 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[9., 22.], [9., 22.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.35], [0.05, 0.09]] elif candidate in (CAR.SONATA, CAR.SONATA_HYBRID): - ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1513. + STD_CARGO_KG ret.wheelbase = 2.84 ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable tire_stiffness_factor = 0.65 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + set_torque_tune(ret.lateralTuning, torque_params['LAT_ACCEL_FACTOR'], torque_params['FRICTION']) elif candidate == CAR.SONATA_LF: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 4497. * CV.LB_TO_KG @@ -98,21 +96,17 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] ret.minSteerSpeed = 32 * CV.MPH_TO_MS elif candidate == CAR.ELANTRA_2021: - ret.lateralTuning.pid.kf = 0.00005 ret.mass = (2800. * CV.LB_TO_KG) + STD_CARGO_KG ret.wheelbase = 2.72 ret.steerRatio = 12.9 tire_stiffness_factor = 0.65 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + set_torque_tune(ret.lateralTuning, torque_params['LAT_ACCEL_FACTOR'], torque_params['FRICTION']) elif candidate == CAR.ELANTRA_HEV_2021: - ret.lateralTuning.pid.kf = 0.00005 ret.mass = (3017. * CV.LB_TO_KG) + STD_CARGO_KG ret.wheelbase = 2.72 ret.steerRatio = 12.9 tire_stiffness_factor = 0.65 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + set_torque_tune(ret.lateralTuning, torque_params['LAT_ACCEL_FACTOR'], torque_params['FRICTION']) elif candidate == CAR.HYUNDAI_GENESIS: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 2060. + STD_CARGO_KG @@ -210,13 +204,12 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.indi.actuatorEffectivenessBP = [0.] ret.lateralTuning.indi.actuatorEffectivenessV = [1.8] elif candidate in (CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_H): - ret.lateralTuning.pid.kf = 0.00005 ret.mass = 3558. * CV.LB_TO_KG ret.wheelbase = 2.80 ret.steerRatio = 13.75 tire_stiffness_factor = 0.5 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + torque_params = CarInterfaceBase.get_torque_params(CAR.KIA_OPTIMA) + set_torque_tune(ret.lateralTuning, torque_params['LAT_ACCEL_FACTOR'], torque_params['FRICTION']) elif candidate == CAR.KIA_STINGER: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1825. + STD_CARGO_KG @@ -258,7 +251,8 @@ class CarInterface(CarInterfaceBase): tire_stiffness_factor = 0.65 ret.maxLateralAccel = 2. - set_torque_tune(ret.lateralTuning, ret.maxLateralAccel, 0.01) + # TODO override until there is more data + set_torque_tune(ret.lateralTuning, 2.0, 0.05) # Genesis elif candidate == CAR.GENESIS_G70: From 25eafa96264aa2f8851b2f0d633de0c7cce3e743 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 13 Jun 2022 18:42:47 -0700 Subject: [PATCH 68/93] Honda Bosch long: fix ACC fault (#24851) Fix Honda bosch long Co-authored-by: redacid95 Co-authored-by: redacid95 --- selfdrive/car/honda/hondacan.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index 9c9e873e18..3d8c79c809 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -115,7 +115,7 @@ def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, idx, stoc } if CP.carFingerprint in HONDA_BOSCH: - acc_hud_values['ACC_ON'] = hud.car != 0 + acc_hud_values['ACC_ON'] = int(enabled) acc_hud_values['FCM_OFF'] = 1 acc_hud_values['FCM_OFF_2'] = 1 else: From dbfe923ecc0e14a781a7f03646ccf885ab2fa47a Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Mon, 13 Jun 2022 19:08:09 -0700 Subject: [PATCH 69/93] Move couple toyotas to torque table values (#24849) * Move couple toyotas to torque table values * Dont set for all cars yet * Dont regress docs * update ref --- selfdrive/car/toyota/interface.py | 24 ++++++++++++------------ selfdrive/test/process_replay/ref_commit | 2 +- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 53accecfe0..9ca9e4e11b 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -2,6 +2,7 @@ from cereal import car from common.conversions import Conversions as CV from panda import Panda +from selfdrive.controls.lib.latcontrol_torque import set_torque_tune from selfdrive.car.toyota.tunes import LatTunes, LongTunes, set_long_tune, set_lat_tune from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, CarControllerParams from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config @@ -32,6 +33,8 @@ class CarInterface(CarInterfaceBase): stop_and_go = False torque_params = CarInterfaceBase.get_torque_params(candidate) + steering_angle_deadzone_deg = 0.0 + set_torque_tune(ret.lateralTuning, torque_params['LAT_ACCEL_FACTOR'], torque_params['FRICTION'], steering_angle_deadzone_deg) if candidate == CAR.PRIUS: stop_and_go = True @@ -39,8 +42,11 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 15.74 # unknown end-to-end spec tire_stiffness_factor = 0.6371 # hand-tune ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG - ret.maxLateralAccel = 1.7 - set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=ret.maxLateralAccel, FRICTION=0.06, steering_angle_deadzone_deg=1.0) + # Only give steer angle deadzone to for bad angle sensor prius + for fw in car_fw: + if fw.ecu == "eps" and not fw.fwVersion == b'8965B47060\x00\x00\x00\x00\x00\x00': + steering_angle_deadzone_deg = 1.0 + set_torque_tune(ret.lateralTuning, torque_params['LAT_ACCEL_FACTOR'], torque_params['FRICTION'], steering_angle_deadzone_deg) elif candidate == CAR.PRIUS_V: stop_and_go = True @@ -48,8 +54,10 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 17.4 tire_stiffness_factor = 0.5533 ret.mass = 3340. * CV.LB_TO_KG + STD_CARGO_KG + # TODO override until there is enough data ret.maxLateralAccel = 1.8 - set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=ret.maxLateralAccel, FRICTION=0.06) + torque_params = CarInterfaceBase.get_torque_params(CAR.PRIUS) + set_torque_tune(ret.lateralTuning, torque_params['LAT_ACCEL_FACTOR'], torque_params['FRICTION'], steering_angle_deadzone_deg) elif candidate in (CAR.RAV4, CAR.RAV4H): stop_and_go = True if (candidate in CAR.RAV4H) else False @@ -57,16 +65,12 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 16.88 # 14.5 is spec end-to-end tire_stiffness_factor = 0.5533 ret.mass = 3650. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - ret.maxLateralAccel = 1.8 - set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=ret.maxLateralAccel, FRICTION=0.06) elif candidate == CAR.COROLLA: ret.wheelbase = 2.70 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 - ret.maxLateralAccel = 2.8 - set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=ret.maxLateralAccel, FRICTION=0.024) elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RXH, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2): stop_and_go = True @@ -91,9 +95,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 13.7 tire_stiffness_factor = 0.7933 ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - if candidate in (CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2): - set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, torque_params['LAT_ACCEL_FACTOR'], torque_params['FRICTION']) - else: + if candidate not in (CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2): set_lat_tune(ret.lateralTuning, LatTunes.PID_C) elif candidate in (CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2): @@ -143,8 +145,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 13.9 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG - ret.maxLateralAccel = 2.0 - set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=ret.maxLateralAccel, FRICTION=0.07) elif candidate in (CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_ESH): stop_and_go = True diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 613b7f26ae..61bbd5cdd1 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -123506cad1877e93bfe5c91ecdce654ef339959b +fe2da24194e3def1823681cc18e7879f24edfc6e From a78197ab1af40dcdc0fe5a8f764b476da3837e87 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 13 Jun 2022 21:49:50 -0700 Subject: [PATCH 70/93] Move RAV4 2022 out of bronze --- docs/CARS.md | 6 +++--- selfdrive/car/toyota/interface.py | 4 ++++ 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/docs/CARS.md b/docs/CARS.md index 479c519dab..9b3322ac8e 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -68,7 +68,7 @@ How We Rate The Cars |Toyota|RAV4 2019-21|All|||||| |Toyota|RAV4 Hybrid 2019-21|All|||||| -# Silver - 75 cars +# Silver - 76 cars |Make|Model|Supported Package|openpilot ACC|Stop and Go|Steer to 0|Steering Torque|Actively Maintained| |---|---|---|:---:|:---:|:---:|:---:|:---:| @@ -130,6 +130,7 @@ How We Rate The Cars |Toyota|Highlander Hybrid 2017-19|All|[3](#footnotes)||||| |Toyota|Prius 2016-20|TSS-P|[3](#footnotes)||||| |Toyota|Prius Prime 2017-20|All|[3](#footnotes)||||| +|Toyota|RAV4 2022|All|||||| |Toyota|RAV4 Hybrid 2016-18|TSS-P|[3](#footnotes)||||| |Toyota|RAV4 Hybrid 2022|All|||||| |Toyota|Sienna 2018-20|All|[3](#footnotes)||||| @@ -148,7 +149,7 @@ How We Rate The Cars |Volkswagen|Taos 2022[7](#footnotes)|Driver Assistance|||||| |Volkswagen|Touran 2017|Driver Assistance|||||| -# Bronze - 70 cars +# Bronze - 69 cars |Make|Model|Supported Package|openpilot ACC|Stop and Go|Steer to 0|Steering Torque|Actively Maintained| |---|---|---|:---:|:---:|:---:|:---:|:---:| @@ -214,7 +215,6 @@ How We Rate The Cars |Toyota|Corolla 2017-19|All|[3](#footnotes)||||| |Toyota|Prius v 2017|TSS-P|[3](#footnotes)||||| |Toyota|RAV4 2016-18|TSS-P|[3](#footnotes)||||| -|Toyota|RAV4 2022|All|||||| |Volkswagen|Arteon 2018, 2021[7](#footnotes)|Driver Assistance|||||| |Volkswagen|California 2021[7](#footnotes)|Driver Assistance|||||| |Volkswagen|Caravelle 2020[7](#footnotes)|Driver Assistance|||||| diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 9ca9e4e11b..7969775a89 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -132,6 +132,10 @@ class CarInterface(CarInterfaceBase): ret.mass = 3585. * CV.LB_TO_KG + STD_CARGO_KG # Average between ICE and Hybrid set_lat_tune(ret.lateralTuning, LatTunes.PID_D) + # TODO: remove once there's data + if candidate == CAR.RAV4_TSS2_2022: + ret.maxLateralAccel = CarInterfaceBase.get_torque_params(CAR.RAV4H_TSS2_2022)['MAX_LAT_ACCEL_MEASURED'] + # 2019+ RAV4 TSS2 uses two different steering racks and specific tuning seems to be necessary. # See https://github.com/commaai/openpilot/pull/21429#issuecomment-873652891 for fw in car_fw: From 1ba8022c184a7e643c2714aaad1aa45e5e68501c Mon Sep 17 00:00:00 2001 From: YassineYousfi Date: Mon, 13 Jun 2022 22:26:31 -0700 Subject: [PATCH 71/93] pin protobuf and hypothesis versions (#24852) pin hypothesis and protobuf --- Pipfile | 3 ++- Pipfile.lock | 58 +++++++++++++++++++++++++++++++++++++--------------- 2 files changed, 44 insertions(+), 17 deletions(-) diff --git a/Pipfile b/Pipfile index 2ffe60e0fe..672692fe40 100644 --- a/Pipfile +++ b/Pipfile @@ -11,7 +11,7 @@ coverage = "*" dictdiffer = "*" fastcluster = "*" hexdump = "*" -hypothesis = "*" +hypothesis = "==6.46.7" inputs = "*" lru-dict = "*" markdown-it-py = "*" @@ -58,6 +58,7 @@ json-rpc = "*" libusb1 = "*" nose = "*" numpy = "*" +protobuf = "==3.20.1" onnx = "*" onnxruntime-gpu = {version = "*", markers="platform_system != 'Darwin'"} pillow = "*" diff --git a/Pipfile.lock b/Pipfile.lock index 25a97890d3..ee0fbc7e84 100644 --- a/Pipfile.lock +++ b/Pipfile.lock @@ -1,7 +1,7 @@ { "_meta": { "hash": { - "sha256": "eb1eeeaabf1266fab353532d10e4d9ae36306a7577f248513909c2d6096ab1c9" + "sha256": "7af354a7ae976b12da1b5f15f0cebdc77595199c11b0bfeb8be9c2827f335c48" }, "pipfile-spec": 6, "requires": { @@ -18,11 +18,11 @@ "default": { "astroid": { "hashes": [ - "sha256:14ffbb4f6aa2cf474a0834014005487f7ecd8924996083ab411e7fa0b508ce0b", - "sha256:f4e4ec5294c4b07ac38bab9ca5ddd3914d4bf46f9006eb5c0ae755755061044e" + "sha256:4f933d0bf5e408b03a6feb5d23793740c27e07340605f236496cd6ce552043d6", + "sha256:ba33a82a9a9c06a5ceed98180c5aab16e29c285b828d94696bf32d6015ea82a9" ], "markers": "python_full_version >= '3.6.2'", - "version": "==2.11.5" + "version": "==2.11.6" }, "atomicwrites": { "hashes": [ @@ -371,7 +371,7 @@ "sha256:6f62d78e2f89b4500b080fe3a81690850cd254227f27f75c3a0c491a1f351ba7", "sha256:e8443a5e7a020e9d7f97f1d7d9cd17c88bcb3bc7e218bf9cf5095fe550be2951" ], - "markers": "python_version < '4' and python_full_version >= '3.6.1'", + "markers": "python_version < '4.0' and python_full_version >= '3.6.1'", "version": "==5.10.1" }, "itsdangerous": { @@ -687,23 +687,47 @@ }, "protobuf": { "hashes": [ + "sha256:06059eb6953ff01e56a25cd02cca1a9649a75a7e65397b5b9b4e929ed71d10cf", + "sha256:097c5d8a9808302fb0da7e20edf0b8d4703274d140fd25c5edabddcde43e081f", "sha256:0d4719e724472e296062ba8e82a36d64693fcfdb550d9dff98af70ca79eafe3d", + "sha256:284f86a6207c897542d7e956eb243a36bb8f9564c1742b253462386e96c6b78f", "sha256:2b35602cb65d53c168c104469e714bf68670335044c38eee3c899d6a8af03ffc", + "sha256:32ca378605b41fd180dfe4e14d3226386d8d1b002ab31c969c366549e66a2bb7", "sha256:32fff501b6df3050936d1839b80ea5899bf34db24792d223d7640611f67de15a", "sha256:34400fd76f85bdae9a2e9c1444ea4699c0280962423eff4418765deceebd81b5", "sha256:3767c64593a49c7ac0accd08ed39ce42744405f0989d468f0097a17496fdbe84", + "sha256:3cc797c9d15d7689ed507b165cd05913acb992d78b379f6014e013f9ecb20996", "sha256:3f2ed842e8ca43b790cb4a101bcf577226e0ded98a6a6ba2d5e12095a08dc4da", "sha256:52c1e44e25f2949be7ffa7c66acbfea940b0945dd416920231f7cb30ea5ac6db", "sha256:5d9b5c8270461706973c3871c6fbdd236b51dfe9dab652f1fb6a36aa88287e47", + "sha256:62f1b5c4cd6c5402b4e2d63804ba49a327e0c386c99b1675c8a0fefda23b2067", + "sha256:69ccfdf3657ba59569c64295b7d51325f91af586f8d5793b734260dfe2e94e2c", + "sha256:6f50601512a3d23625d8a85b1638d914a0970f17920ff39cec63aaef80a93fb7", "sha256:72d357cc4d834cc85bd957e8b8e1f4b64c2eac9ca1a942efeb8eb2e723fca852", + "sha256:7403941f6d0992d40161aa8bb23e12575637008a5a02283a930addc0508982f9", + "sha256:755f3aee41354ae395e104d62119cb223339a8f3276a0cd009ffabfcdd46bb0c", + "sha256:77053d28427a29987ca9caf7b72ccafee011257561259faba8dd308fda9a8739", "sha256:79cd8d0a269b714f6b32641f86928c718e8d234466919b3f552bfb069dbb159b", + "sha256:7e371f10abe57cee5021797126c93479f59fccc9693dafd6bd5633ab67808a91", + "sha256:9016d01c91e8e625141d24ec1b20fed584703e527d28512aa8c8707f105a683c", + "sha256:9be73ad47579abc26c12024239d3540e6b765182a91dbc88e23658ab71767153", "sha256:a4c0c6f2f95a559e59a0258d3e4b186f340cbdc5adec5ce1bc06d01972527c88", + "sha256:adc31566d027f45efe3f44eeb5b1f329da43891634d61c75a5944e9be6dd42c9", + "sha256:adfc6cf69c7f8c50fd24c793964eef18f0ac321315439d94945820612849c388", + "sha256:af0ebadc74e281a517141daad9d0f2c5d93ab78e9d455113719a45a49da9db4e", "sha256:b309fda192850ac4184ca1777aab9655564bc8d10a9cc98f10e1c8bf11295c22", "sha256:b3d7d4b4945fe3c001403b6c24442901a5e58c0a3059290f5a63523ed4435f82", - "sha256:c8829092c5aeb61619161269b2f8a2e36fd7cb26abbd9282d3bc453f02769146" + "sha256:c8829092c5aeb61619161269b2f8a2e36fd7cb26abbd9282d3bc453f02769146", + "sha256:cb29edb9eab15742d791e1025dd7b6a8f6fcb53802ad2f6e3adcb102051063ab", + "sha256:cd68be2559e2a3b84f517fb029ee611546f7812b1fdd0aa2ecc9bc6ec0e4fdde", + "sha256:cdee09140e1cd184ba9324ec1df410e7147242b94b5f8b0c64fc89e38a8ba531", + "sha256:db977c4ca738dd9ce508557d4fce0f5aebd105e158c725beec86feb1f6bc20d8", + "sha256:dd5789b2948ca702c17027c84c2accb552fc30f4622a98ab5c51fcfe8c50d3e7", + "sha256:e250a42f15bf9d5b09fe1b293bdba2801cd520a9f5ea2d7fb7536d4441811d20", + "sha256:ff8d8fa42675249bb456f5db06c00de6c2f4c27a065955917b28c4f15978b9c3" ], - "markers": "python_version >= '3.7'", - "version": "==4.21.1" + "index": "pypi", + "version": "==3.20.1" }, "psutil": { "hashes": [ @@ -1155,11 +1179,11 @@ }, "setuptools": { "hashes": [ - "sha256:1f5d3a1502812025cdb2e5609b6af2d207332e3f50febe6db10ed3a59b2f155f", - "sha256:30b6b0fbacc459c90d27a63e6173facfc8b8c99a48fb24b5044f459ba63cd6cf" + "sha256:5a844ad6e190dccc67d6d7411d119c5152ce01f7c76be4d8a1eaa314501bba77", + "sha256:bf8a748ac98b09d32c9a64a995a6b25921c96cc5743c1efa82763ba80ff54e91" ], "markers": "python_version >= '3.7'", - "version": "==62.3.4" + "version": "==62.4.0" }, "six": { "hashes": [ @@ -1206,7 +1230,7 @@ "sha256:0f4050db66fd445b885778900ce4dd9aea8c90c4721141fde0d6ade893820ef1", "sha256:71ceb10c0eefd8b8f11fe34e8a51ad07812cb1dc3de23247425fbc9ddc47b9dd" ], - "markers": "python_version >= '3.6' and python_version < '4'", + "markers": "python_version >= '3.6' and python_version < '4.0'", "version": "==0.11.0" }, "tqdm": { @@ -1222,7 +1246,7 @@ "sha256:6657594ee297170d19f67d55c05852a874e7eb634f4f753dbd667855e07c1708", "sha256:f1c24655a0da0d1b67f07e17a5e6b2a105894e6824b92096378bb3668ef02376" ], - "markers": "python_version < '3.10'", + "markers": "python_version >= '3.7'", "version": "==4.2.0" }, "urllib3": { @@ -1733,11 +1757,13 @@ }, "hypothesis": { "hashes": [ + "sha256:2696cdb9005946bf1d2b215cc91d3fc01625e3342eb8743ddd04b667b2f1882b", "sha256:4ad26c5d434171ffc02aba569dd52255573d615554c062bc30734dbe6f318c61", - "sha256:69978811f1d9c19710c7d2bf8233dc43c80efa964251b72efbe8274044e073b4" + "sha256:69978811f1d9c19710c7d2bf8233dc43c80efa964251b72efbe8274044e073b4", + "sha256:967009fa561b3a3f8363a73d71923357271c37dc7fa27b30c2d21a1b6092b240" ], "index": "pypi", - "version": "==6.47.1" + "version": "==6.46.7" }, "identify": { "hashes": [ @@ -2546,7 +2572,7 @@ "sha256:6657594ee297170d19f67d55c05852a874e7eb634f4f753dbd667855e07c1708", "sha256:f1c24655a0da0d1b67f07e17a5e6b2a105894e6824b92096378bb3668ef02376" ], - "markers": "python_version < '3.10'", + "markers": "python_version >= '3.7'", "version": "==4.2.0" }, "urllib3": { From a9038bc3fd0ed12b441804923c46927b5f6bbbfe Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 13 Jun 2022 22:27:44 -0700 Subject: [PATCH 72/93] bump opendbc (#24853) --- opendbc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc b/opendbc index 58a2c9b2fc..a7b391cce8 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 58a2c9b2fc5c60ca68d3dced09f6c4c72ca72415 +Subproject commit a7b391cce88908824f1417f0c7abd35e3ae16f96 From 170ed3d761a58222a626c86b9706f9be4900d058 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 14 Jun 2022 12:03:30 -0700 Subject: [PATCH 73/93] process replay: clean up common code (#24855) * regen and process replay clean up * test_fuzzy actually uses fingerprint hardcoding fix * revert * revert * this can be a url or path so just print full variable --- .../test/process_replay/process_replay.py | 35 ++++++++----------- selfdrive/test/process_replay/regen.py | 19 +--------- selfdrive/test/process_replay/regen_all.py | 14 ++++---- .../test/process_replay/test_processes.py | 3 +- 4 files changed, 24 insertions(+), 47 deletions(-) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 1eda9bc7f5..363035ab22 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -347,7 +347,7 @@ def replay_process(cfg, lr, fingerprint=None): else: return cpp_replay_process(cfg, lr, fingerprint) -def setup_env(simulation=False): +def setup_env(simulation=False, CP=None): params = Params() params.clear_all() params.put_bool("OpenpilotEnabledToggle", True) @@ -358,12 +358,22 @@ def setup_env(simulation=False): os.environ["NO_RADAR_SLEEP"] = "1" os.environ["REPLAY"] = "1" + os.environ['SKIP_FW_QUERY'] = "" + os.environ['FINGERPRINT'] = "" if simulation: os.environ["SIMULATION"] = "1" elif "SIMULATION" in os.environ: del os.environ["SIMULATION"] + # Regen or python process + if CP is not None: + if CP.fingerprintSource == "fw" and CP.carFingerprint in FW_VERSIONS: + params.put("CarParamsCache", CP.as_builder().to_bytes()) + else: + os.environ['SKIP_FW_QUERY'] = "1" + os.environ['FINGERPRINT'] = CP.carFingerprint + def python_replay_process(cfg, lr, fingerprint=None): sub_sockets = [s for _, sub in cfg.pub_sub.items() for s in sub] pub_sockets = [s for s in cfg.pub_sub.keys() if s != 'can'] @@ -378,30 +388,13 @@ def python_replay_process(cfg, lr, fingerprint=None): all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime) pub_msgs = [msg for msg in all_msgs if msg.which() in list(cfg.pub_sub.keys())] - setup_env() - - # TODO: remove after getting new route for civic & accord - migration = { - "HONDA CIVIC 2016 TOURING": "HONDA CIVIC 2016", - "HONDA ACCORD 2018 SPORT 2T": "HONDA ACCORD 2018", - "HONDA ACCORD 2T 2018": "HONDA ACCORD 2018", - "Mazda CX-9 2021": "MAZDA CX-9 2021", - } - if fingerprint is not None: os.environ['SKIP_FW_QUERY'] = "1" os.environ['FINGERPRINT'] = fingerprint + setup_env() else: - os.environ['SKIP_FW_QUERY'] = "" - os.environ['FINGERPRINT'] = "" - for msg in lr: - if msg.which() == 'carParams': - car_fingerprint = migration.get(msg.carParams.carFingerprint, msg.carParams.carFingerprint) - if msg.carParams.fingerprintSource == "fw" and (car_fingerprint in FW_VERSIONS): - Params().put("CarParamsCache", msg.carParams.as_builder().to_bytes()) - else: - os.environ['SKIP_FW_QUERY'] = "1" - os.environ['FINGERPRINT'] = car_fingerprint + CP = [m for m in lr if m.which() == 'carParams'][0].carParams + setup_env(CP=CP) assert(type(managed_processes[cfg.proc_name]) is PythonProcess) managed_processes[cfg.proc_name].prepare() diff --git a/selfdrive/test/process_replay/regen.py b/selfdrive/test/process_replay/regen.py index 653efaf32c..dfc62a4558 100755 --- a/selfdrive/test/process_replay/regen.py +++ b/selfdrive/test/process_replay/regen.py @@ -15,7 +15,6 @@ from cereal.visionipc import VisionIpcServer, VisionStreamType from common.params import Params from common.realtime import Ratekeeper, DT_MDL, DT_DMON, sec_since_boot from common.transformations.camera import eon_f_frame_size, eon_d_frame_size, tici_f_frame_size, tici_d_frame_size -from selfdrive.car.fingerprints import FW_VERSIONS from selfdrive.manager.process import ensure_running from selfdrive.manager.process_config import managed_processes from selfdrive.test.process_replay.process_replay import FAKEDATA, setup_env, check_enabled @@ -181,28 +180,12 @@ def regen_segment(lr, frs=None, outdir=FAKEDATA, disable_tqdm=False): if frs is None: frs = dict() - setup_env() params = Params() - os.environ["LOG_ROOT"] = outdir - os.environ['SKIP_FW_QUERY'] = "" - os.environ['FINGERPRINT'] = "" - - # TODO: remove after getting new route for Mazda - fp_migration = { - "Mazda CX-9 2021": "MAZDA CX-9 2021", - } - # TODO: remove after getting new route for Subaru - fingerprint_problem = ["SUBARU IMPREZA LIMITED 2019"] for msg in lr: if msg.which() == 'carParams': - car_fingerprint = fp_migration.get(msg.carParams.carFingerprint, msg.carParams.carFingerprint) - if len(msg.carParams.carFw) and (car_fingerprint in FW_VERSIONS) and (car_fingerprint not in fingerprint_problem): - params.put("CarParamsCache", msg.carParams.as_builder().to_bytes()) - else: - os.environ['SKIP_FW_QUERY'] = "1" - os.environ['FINGERPRINT'] = car_fingerprint + setup_env(CP=msg.carParams) elif msg.which() == 'liveCalibration': params.put("CalibrationParams", msg.as_builder().to_bytes()) diff --git a/selfdrive/test/process_replay/regen_all.py b/selfdrive/test/process_replay/regen_all.py index 765e5c3b68..c3ea1d41e1 100755 --- a/selfdrive/test/process_replay/regen_all.py +++ b/selfdrive/test/process_replay/regen_all.py @@ -8,26 +8,28 @@ from tqdm import tqdm from selfdrive.test.process_replay.helpers import OpenpilotPrefix from selfdrive.test.process_replay.regen import regen_and_save from selfdrive.test.process_replay.test_processes import FAKEDATA, original_segments as segments +from tools.lib.route import SegmentName -def regen_job(segment): + +def regen_job(segment, disable_tqdm): with OpenpilotPrefix(): - route = segment[1].rsplit('--', 1)[0] - sidx = int(segment[1].rsplit('--', 1)[1]) - fake_dongle_id = 'regen' + ''.join(random.choice('0123456789ABCDEF') for i in range(11)) + sn = SegmentName(segment[1]) + fake_dongle_id = 'regen' + ''.join(random.choice('0123456789ABCDEF') for _ in range(11)) try: - relr = regen_and_save(route, sidx, upload=True, use_route_meta=False, outdir=os.path.join(FAKEDATA, fake_dongle_id), disable_tqdm=True) + relr = regen_and_save(sn.route_name.canonical_name, sn.segment_num, upload=True, use_route_meta=False, outdir=os.path.join(FAKEDATA, fake_dongle_id), disable_tqdm=disable_tqdm) relr = '|'.join(relr.split('/')[-2:]) return f' ("{segment[0]}", "{relr}"), ' except Exception as e: return f" {segment} failed: {str(e)}" + if __name__ == "__main__": parser = argparse.ArgumentParser(description="Generate new segments from old ones") parser.add_argument("-j", "--jobs", type=int, default=1) args = parser.parse_args() with concurrent.futures.ProcessPoolExecutor(max_workers=args.jobs) as pool: - p = list(pool.map(regen_job, segments)) + p = list(pool.map(regen_job, segments, [args.jobs > 1] * args.jobs)) msg = "Copy these new segments into test_processes.py:" for seg in tqdm(p, desc="Generating segments"): msg += "\n" + str(seg) diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 9f83a08e23..25fbd210cc 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -93,8 +93,7 @@ def test_process(cfg, lr, ref_log_path, ignore_fields=None, ignore_msgs=None): # check to make sure openpilot is engaged in the route if cfg.proc_name == "controlsd": if not check_enabled(log_msgs): - segment = os.path.basename(ref_log_path).split("/")[-1].split("_")[0] - raise Exception(f"Route never enabled: {segment}") + raise Exception(f"Route never enabled: {ref_log_path}") try: return compare_logs(ref_log_msgs, log_msgs, ignore_fields + cfg.ignore, ignore_msgs, cfg.tolerance), log_msgs From 7a6f57a28e6efda72319fc0b2a4873bcf7e06829 Mon Sep 17 00:00:00 2001 From: George Hotz <72895+geohot@users.noreply.github.com> Date: Tue, 14 Jun 2022 15:46:03 -0700 Subject: [PATCH 74/93] remove weights fixup with new SNPE (#24254) * remove weights fixup with new SNPE * Update ref Co-authored-by: Comma Device Co-authored-by: Harald Schafer --- selfdrive/modeld/SConscript | 12 ++---------- .../test/process_replay/model_replay_ref_commit | 2 +- 2 files changed, 3 insertions(+), 11 deletions(-) diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index 1f1c661c8b..3e9738d864 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -65,22 +65,14 @@ common_model = lenv.Object(common_src) if use_thneed and arch == "larch64": fn = File("models/supercombo").abspath compiler = lenv.Program('thneed/compile', ["thneed/compile.cc"]+common_model, LIBS=libs) - cmd = f"cd {Dir('.').abspath} && {compiler[0].abspath} --in {fn}.dlc --out {fn}_badweights.thneed --binary --optimize" + cmd = f"cd {Dir('.').abspath} && {compiler[0].abspath} --in {fn}.dlc --out {fn}.thneed --binary --optimize" lib_paths = ':'.join(Dir(p).abspath for p in lenv["LIBPATH"]) kernel_path = os.path.join(Dir('.').abspath, "thneed", "kernels") cenv = Environment(ENV={'LD_LIBRARY_PATH': f"{lib_paths}:{lenv['ENV']['LD_LIBRARY_PATH']}", 'KERNEL_PATH': kernel_path}) kernels = [os.path.join(kernel_path, x) for x in os.listdir(kernel_path) if x.endswith(".cl")] - cenv.Command(fn + "_badweights.thneed", [fn + ".dlc", kernels, compiler], cmd) - - from selfdrive.modeld.thneed.weights_fixup import weights_fixup - def weights_fixup_action(target, source, env): - weights_fixup(target[0].abspath, source[0].abspath, source[1].abspath) - - env = Environment(BUILDERS = {'WeightFixup' : Builder(action = weights_fixup_action)}) - env.WeightFixup(target=fn + ".thneed", source=[fn+"_badweights.thneed", fn+".dlc"]) - + cenv.Command(fn + ".thneed", [fn + ".dlc", kernels, compiler], cmd) lenv.Program('_dmonitoringmodeld', [ "dmonitoringmodeld.cc", diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index 90520f2619..a7a909d2a4 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -f74ab97371be93fdc28333e5ea12bbb78c3a32d0 +512a9d4596c8faba304d6f7ded2ce77837357b65 From f8f4337fb3c50d6c125f59a8f59c93818fe71994 Mon Sep 17 00:00:00 2001 From: Jason Shuler Date: Tue, 14 Jun 2022 20:28:15 -0400 Subject: [PATCH 75/93] GM: add support for vehicles with manual parking brakes (#24766) Switch to general park brake signal --- selfdrive/car/gm/carstate.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index e6a1c08dda..48b9f25444 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -64,7 +64,7 @@ class CarState(CarStateBase): ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 1 ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2 - ret.parkingBrake = pt_cp.vl["EPBStatus"]["EPBClosed"] == 1 + ret.parkingBrake = pt_cp.vl["VehicleIgnitionAlt"]["ParkBrake"] == 1 ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0 ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1 ret.accFaulted = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.FAULTED @@ -100,7 +100,7 @@ class CarState(CarStateBase): ("LKATorqueDelivered", "PSCMStatus"), ("LKATorqueDeliveredStatus", "PSCMStatus"), ("TractionControlOn", "ESPStatus"), - ("EPBClosed", "EPBStatus"), + ("ParkBrake", "VehicleIgnitionAlt"), ("CruiseMainOn", "ECMEngineStatus"), ] @@ -110,7 +110,7 @@ class CarState(CarStateBase): ("PSCMStatus", 10), ("ESPStatus", 10), ("BCMDoorBeltStatus", 10), - ("EPBStatus", 20), + ("VehicleIgnitionAlt", 10), ("EBCMWheelSpdFront", 20), ("EBCMWheelSpdRear", 20), ("AcceleratorPedal2", 33), From 6757e235f92b72741f2f4375548fde883ede324d Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Tue, 14 Jun 2022 20:38:25 -0700 Subject: [PATCH 76/93] little zookeeper fixes (#24861) * little zookeeper fixes * bump that up --- Pipfile | 1 + Pipfile.lock | 135 ++++++++++++++++++------------- tools/zookeeper/__init__.py | 7 +- tools/zookeeper/power_monitor.py | 28 +++---- tools/zookeeper/requirements.txt | 1 - 5 files changed, 98 insertions(+), 74 deletions(-) mode change 100755 => 100644 tools/zookeeper/__init__.py delete mode 100644 tools/zookeeper/requirements.txt diff --git a/Pipfile b/Pipfile index 672692fe40..b8545b1a21 100644 --- a/Pipfile +++ b/Pipfile @@ -40,6 +40,7 @@ subprocess32 = "*" tenacity = "*" mpld3 = "*" carla = {version = "==0.9.13", markers="platform_system != 'Darwin'"} +ft4222 = "*" [packages] atomicwrites = "*" diff --git a/Pipfile.lock b/Pipfile.lock index ee0fbc7e84..cc347a60f4 100644 --- a/Pipfile.lock +++ b/Pipfile.lock @@ -1,7 +1,7 @@ { "_meta": { "hash": { - "sha256": "7af354a7ae976b12da1b5f15f0cebdc77595199c11b0bfeb8be9c2827f335c48" + "sha256": "d2629168f477b3a14f68f26e3b63ea9797b20599c066b2aa23a027bcee2ca40a" }, "pipfile-spec": 6, "requires": { @@ -146,7 +146,7 @@ "sha256:2857e29ff0d34db842cd7ca3230549d1a697f96ee6d3fb071cfa6c7393832597", "sha256:6881edbebdb17b39b4eaaa821b438bf6eddffb4468cf344f09f89def34a8b1df" ], - "markers": "python_version >= '3.5'", + "markers": "python_full_version >= '3.5.0'", "version": "==2.0.12" }, "click": { @@ -347,7 +347,7 @@ "sha256:84d9dd047ffa80596e0f246e2eab0b391788b0503584e8945f2368256d2735ff", "sha256:9d643ff0a55b762d5cdb124b8eaa99c66322e2157b69160bc32796e824360e6d" ], - "markers": "python_version >= '3.5'", + "markers": "python_full_version >= '3.5.0'", "version": "==3.3" }, "importlib-metadata": { @@ -1246,7 +1246,7 @@ "sha256:6657594ee297170d19f67d55c05852a874e7eb634f4f753dbd667855e07c1708", "sha256:f1c24655a0da0d1b67f07e17a5e6b2a105894e6824b92096378bb3668ef02376" ], - "markers": "python_version >= '3.7'", + "markers": "python_version < '3.10'", "version": "==4.2.0" }, "urllib3": { @@ -1443,11 +1443,11 @@ }, "babel": { "hashes": [ - "sha256:3f349e85ad3154559ac4930c3918247d319f21910d5ce4b25d439ed8693b98d2", - "sha256:98aeaca086133efb3e1e2aad0396987490c8425929ddbcfe0550184fdc54cd13" + "sha256:7aed055f0c04c9e7f51a2f75261e41e1c804efa724cb65b60a970dd4448d469d", + "sha256:81a3beca4d0cd40a9cfb9e2adb2cf39261c2f959b92e7a74750befe5d79afd7b" ], "markers": "python_version >= '3.6'", - "version": "==2.10.1" + "version": "==2.10.2" }, "bcrypt": { "hashes": [ @@ -1565,7 +1565,7 @@ "sha256:2857e29ff0d34db842cd7ca3230549d1a697f96ee6d3fb071cfa6c7393832597", "sha256:6881edbebdb17b39b4eaaa821b438bf6eddffb4468cf344f09f89def34a8b1df" ], - "markers": "python_version >= '3.5'", + "markers": "python_full_version >= '3.5.0'", "version": "==2.0.12" }, "control": { @@ -1748,6 +1748,33 @@ "markers": "python_version >= '3.7'", "version": "==4.33.3" }, + "ft4222": { + "hashes": [ + "sha256:1489b08e4042cb2b24894495c1c8514fa115122e9f8a739f665f0e4d8c53d3f4", + "sha256:1c71913f9fb862634fb77eb6efeea38b2b839e09f739aee5864b9549bcf1c4a9", + "sha256:1f9be0961bd7f3e0c1729c9692f602244e93898610611602ce1fa16059ac5bfa", + "sha256:208cde748fc2bf4a753e217ee2844e75d7d1b6d370a8937a2055f9de8906f933", + "sha256:372be3d7d04c0f6dfa62ff66b3d4be5c39d57c258ab562d9eb0d7cdd3a90ed0f", + "sha256:3ebf9380315c6af8f9f3bc5c5c7a5ae06498349c14a7b2e65d5067b20462d21b", + "sha256:422740efac86f3fdd971bb9a94d9974e56cfe062284a2be1a85db96dd0e77e5e", + "sha256:4414ef82a6321c5205ab0e3dab2bf072f14b4dd3262e5f392560adf107210d41", + "sha256:459894dcca7db71a0d58e6f3abb4c9dd67654c92ad9d934f1e32dc08bed87645", + "sha256:4b7e4c8b5e4a8fe769127d3db99b7b410468ffe2718a07f7b317e7dc1ad9ca4f", + "sha256:68e6b357ea2287fe0e8dac287efbac2f0ce9632d65e828d422d3a0604555c174", + "sha256:800a26a4a8fc854f41e4de41e45b6deafdeb78ae5eff48818f710bb2d94a8c11", + "sha256:8790e96b4c3f8d1fb768f689f1c437a59b20889e4726ed7457ff3d188f0a3274", + "sha256:9369b55393b2e1f58f8b8516bcf9cd6fd34c05e77377c4cc48be39a49cad7be5", + "sha256:a6236f05b8948cd9c113c7159e3fdde202a0f73dce6440da078fd2fc9e411123", + "sha256:b7f58cd16213a5c92503c530b301f071d67283c80c1e8cb43d6f3ec5c186df35", + "sha256:cc368b06b92529a11add37d14196d2e2aaae19c6ffc51b9457513a0d05ceae1a", + "sha256:d479c037b417ff289727112f1d4725563b78448d3765f457c8bf6884e4d83abd", + "sha256:dce00d513be811f738954c5593c52b533a02331e8a26983280dea3f4d864962f", + "sha256:eadcbc1dd20ed6d7d07dc53354ea137bcea30fb0889d74bedcd189cdb4f61c84", + "sha256:ec984bd7e9300e5f2e50823dcd5874d54aa3e7503e69afe7eb3b4cea77071084" + ], + "index": "pypi", + "version": "==1.4.1" + }, "hexdump": { "hashes": [ "sha256:d781a43b0c16ace3f9366aade73e8ad3a7bd5137d58f0b45ab2d3f54876f20db" @@ -1778,7 +1805,7 @@ "sha256:84d9dd047ffa80596e0f246e2eab0b391788b0503584e8945f2368256d2735ff", "sha256:9d643ff0a55b762d5cdb124b8eaa99c66322e2157b69160bc32796e824360e6d" ], - "markers": "python_version >= '3.5'", + "markers": "python_full_version >= '3.5.0'", "version": "==3.3" }, "imagesize": { @@ -1822,52 +1849,52 @@ }, "kiwisolver": { "hashes": [ - "sha256:0b7f50a1a25361da3440f07c58cd1d79957c2244209e4f166990e770256b6b0b", - "sha256:0c380bb5ae20d829c1a5473cfcae64267b73aaa4060adc091f6df1743784aae0", - "sha256:0d98dca86f77b851350c250f0149aa5852b36572514d20feeadd3c6b1efe38d0", - "sha256:0e45e780a74416ef2f173189ef4387e44b5494f45e290bcb1f03735faa6779bf", - "sha256:0e8afdf533b613122e4bbaf3c1e42c2a5e9e2d1dd3a0a017749a7658757cb377", - "sha256:1008346a7741620ab9cc6c96e8ad9b46f7a74ce839dbb8805ddf6b119d5fc6c2", - "sha256:1d1078ba770d6165abed3d9a1be1f9e79b61515de1dd00d942fa53bba79f01ae", - "sha256:1dcade8f6fe12a2bb4efe2cbe22116556e3b6899728d3b2a0d3b367db323eacc", - "sha256:240009fdf4fa87844f805e23f48995537a8cb8f8c361e35fda6b5ac97fcb906f", - "sha256:240c2d51d098395c012ddbcb9bd7b3ba5de412a1d11840698859f51d0e643c4f", - "sha256:262c248c60f22c2b547683ad521e8a3db5909c71f679b93876921549107a0c24", - "sha256:2e6cda72db409eefad6b021e8a4f964965a629f577812afc7860c69df7bdb84a", - "sha256:3c032c41ae4c3a321b43a3650e6ecc7406b99ff3e5279f24c9b310f41bc98479", - "sha256:42f6ef9b640deb6f7d438e0a371aedd8bef6ddfde30683491b2e6f568b4e884e", - "sha256:484f2a5f0307bc944bc79db235f41048bae4106ffa764168a068d88b644b305d", - "sha256:69b2d6c12f2ad5f55104a36a356192cfb680c049fe5e7c1f6620fc37f119cdc2", - "sha256:6e395ece147f0692ca7cdb05a028d31b83b72c369f7b4a2c1798f4b96af1e3d8", - "sha256:6ece2e12e4b57bc5646b354f436416cd2a6f090c1dadcd92b0ca4542190d7190", - "sha256:71469b5845b9876b8d3d252e201bef6f47bf7456804d2fbe9a1d6e19e78a1e65", - "sha256:7f606d91b8a8816be476513a77fd30abe66227039bd6f8b406c348cb0247dcc9", - "sha256:7f88c4b8e449908eeddb3bbd4242bd4dc2c7a15a7aa44bb33df893203f02dc2d", - "sha256:81237957b15469ea9151ec8ca08ce05656090ffabc476a752ef5ad7e2644c526", - "sha256:89b57c2984f4464840e4b768affeff6b6809c6150d1166938ade3e22fbe22db8", - "sha256:8a830a03970c462d1a2311c90e05679da56d3bd8e78a4ba9985cb78ef7836c9f", - "sha256:8ae5a071185f1a93777c79a9a1e67ac46544d4607f18d07131eece08d415083a", - "sha256:8b6086aa6936865962b2cee0e7aaecf01ab6778ce099288354a7229b4d9f1408", - "sha256:8ec2e55bf31b43aabe32089125dca3b46fdfe9f50afbf0756ae11e14c97b80ca", - "sha256:8ff3033e43e7ca1389ee59fb7ecb8303abb8713c008a1da49b00869e92e3dd7c", - "sha256:91eb4916271655dfe3a952249cb37a5c00b6ba68b4417ee15af9ba549b5ba61d", - "sha256:9d2bb56309fb75a811d81ed55fbe2208aa77a3a09ff5f546ca95e7bb5fac6eff", - "sha256:a4e8f072db1d6fb7a7cc05a6dbef8442c93001f4bb604f1081d8c2db3ca97159", - "sha256:b1605c7c38cc6a85212dfd6a641f3905a33412e49f7c003f35f9ac6d71f67720", - "sha256:b3e251e5c38ac623c5d786adb21477f018712f8c6fa54781bd38aa1c60b60fc2", - "sha256:b978afdb913ca953cf128d57181da2e8798e8b6153be866ae2a9c446c6162f40", - "sha256:be9a650890fb60393e60aacb65878c4a38bb334720aa5ecb1c13d0dac54dd73b", - "sha256:c222f91a45da9e01a9bc4f760727ae49050f8e8345c4ff6525495f7a164c8973", - "sha256:c839bf28e45d7ddad4ae8f986928dbf5a6d42ff79760d54ec8ada8fb263e097c", - "sha256:cbb5eb4a2ea1ffec26268d49766cafa8f957fe5c1b41ad00733763fae77f9436", - "sha256:e348f1904a4fab4153407f7ccc27e43b2a139752e8acf12e6640ba683093dd96", - "sha256:e677cc3626287f343de751e11b1e8a5b915a6ac897e8aecdbc996cd34de753a0", - "sha256:f74f2a13af201559e3d32b9ddfc303c94ae63d63d7f4326d06ce6fe67e7a8255", - "sha256:fa4d97d7d2b2c082e67907c0b8d9f31b85aa5d3ba0d33096b7116f03f8061261", - "sha256:ffbdb9a96c536f0405895b5e21ee39ec579cb0ed97bdbd169ae2b55f41d73219" + "sha256:007799c7fa934646318fc128b033bb6e6baabe7fbad521bfb2279aac26225cd7", + "sha256:130c6c35eded399d3967cf8a542c20b671f5ba85bd6f210f8b939f868360e9eb", + "sha256:1858ad3cb686eccc7c6b7c5eac846a1cfd45aacb5811b2cf575e80b208f5622a", + "sha256:1ae7aa0784aeadfbd693c27993727792fbe1455b84d49970bad5886b42976b18", + "sha256:1d2c744aeedce22c122bb42d176b4aa6d063202a05a4abdacb3e413c214b3694", + "sha256:21a3a98f0a21fc602663ca9bce2b12a4114891bdeba2dea1e9ad84db59892fca", + "sha256:22ccba48abae827a0f952a78a7b1a7ff01866131e5bbe1f826ce9bda406bf051", + "sha256:26b5a70bdab09e6a2f40babc4f8f992e3771751e144bda1938084c70d3001c09", + "sha256:2d76780d9c65c7529cedd49fa4802d713e60798d8dc3b0d5b12a0a8f38cca51c", + "sha256:325fa1b15098e44fe4590a6c5c09a212ca10c6ebb5d96f7447d675f6c8340e4e", + "sha256:3a297d77b3d6979693f5948df02b89431ae3645ec95865e351fb45578031bdae", + "sha256:3b1dcbc49923ac3c973184a82832e1f018dec643b1e054867d04a3a22255ec6a", + "sha256:40240da438c0ebfe2aa76dd04b844effac6679423df61adbe3437d32f23468d9", + "sha256:46c6e5018ba31d5ee7582f323d8661498a154dea1117486a571db4c244531f24", + "sha256:46fb56fde006b7ef5f8eaa3698299b0ea47444238b869ff3ced1426aa9fedcb5", + "sha256:4dc350cb65fe4e3f737d50f0465fa6ea0dcae0e5722b7edf5d5b0a0e3cd2c3c7", + "sha256:51078855a16b7a4984ed2067b54e35803d18bca9861cb60c60f6234b50869a56", + "sha256:547111ef7cf13d73546c2de97ce434935626c897bdec96a578ca100b5fcd694b", + "sha256:5fb73cc8a34baba1dfa546ae83b9c248ef6150c238b06fc53d2773685b67ec67", + "sha256:654280c5f41831ddcc5a331c0e3ce2e480bbc3d7c93c18ecf6236313aae2d61a", + "sha256:6b3136eecf7e1b4a4d23e4b19d6c4e7a8e0b42d55f30444e3c529700cdacaa0d", + "sha256:7118ca592d25b2957ff7b662bc0fe4f4c2b5d5b27814b9b1bc9f2fb249a970e7", + "sha256:71af5b43e4fa286a35110fc5bb740fdeae2b36ca79fbcf0a54237485baeee8be", + "sha256:747190fcdadc377263223f8f72b038381b3b549a8a3df5baf4d067da4749b046", + "sha256:8395064d63b26947fa2c9faeea9c3eee35e52148c5339c37987e1d96fbf009b3", + "sha256:84f85adfebd7d3c3db649efdf73659e1677a2cf3fa6e2556a3f373578af14bf7", + "sha256:86bcf0009f2012847a688f2f4f9b16203ca4c835979a02549aa0595d9f457cc8", + "sha256:ab8a15c2750ae8d53e31f77a94f846d0a00772240f1c12817411fa2344351f86", + "sha256:af24b21c2283ca69c416a8a42cde9764dc36c63d3389645d28c69b0e93db3cd7", + "sha256:afe173ac2646c2636305ab820cc0380b22a00a7bca4290452e7166b4f4fa49d0", + "sha256:b9eb88593159a53a5ee0b0159daee531ff7dd9c87fa78f5d807ca059c7eb1b2b", + "sha256:c16635f8dddbeb1b827977d0b00d07b644b040aeb9ff8607a9fc0997afa3e567", + "sha256:ca3eefb02ef17257fae8b8555c85e7c1efdfd777f671384b0e4ef27409b02720", + "sha256:caa59e2cae0e23b1e225447d7a9ddb0f982f42a6a22d497a484dfe62a06f7c0e", + "sha256:cb55258931448d61e2d50187de4ee66fc9d9f34908b524949b8b2b93d0c57136", + "sha256:d248c46c0aa406695bda2abf99632db991f8b3a6d46018721a2892312a99f069", + "sha256:d2578e5149ff49878934debfacf5c743fab49eca5ecdb983d0b218e1e554c498", + "sha256:dd22085446f3eca990d12a0878eeb5199dc9553b2e71716bfe7bed9915a472ab", + "sha256:e7cf940af5fee00a92e281eb157abe8770227a5255207818ea9a34e54a29f5b2", + "sha256:f70f3d028794e31cf9d1a822914efc935aadb2438ec4e8d4871d95eb1ce032d6", + "sha256:fd2842a0faed9ab9aba0922c951906132d9384be89690570f0ed18cd4f20e658", + "sha256:fd628e63ffdba0112e3ddf1b1e9f3db29dd8262345138e08f4938acbc6d0805a", + "sha256:ffd7cf165ff71afb202b3f36daafbf298932bee325aac9f58e1c9cd55838bef0" ], "markers": "python_version >= '3.7'", - "version": "==1.4.2" + "version": "==1.4.3" }, "lru-dict": { "hashes": [ @@ -2572,7 +2599,7 @@ "sha256:6657594ee297170d19f67d55c05852a874e7eb634f4f753dbd667855e07c1708", "sha256:f1c24655a0da0d1b67f07e17a5e6b2a105894e6824b92096378bb3668ef02376" ], - "markers": "python_version >= '3.7'", + "markers": "python_version < '3.10'", "version": "==4.2.0" }, "urllib3": { diff --git a/tools/zookeeper/__init__.py b/tools/zookeeper/__init__.py old mode 100755 new mode 100644 index cd64d3a1fb..42438cb209 --- a/tools/zookeeper/__init__.py +++ b/tools/zookeeper/__init__.py @@ -1,9 +1,6 @@ #!/usr/bin/env python3 - -# Python library to control Zookeeper - -import ft4222 # pylint: disable=import-error -import ft4222.I2CMaster # pylint: disable=import-error +import ft4222 +import ft4222.I2CMaster DEBUG = False diff --git a/tools/zookeeper/power_monitor.py b/tools/zookeeper/power_monitor.py index f2796bad31..d3bdd6679f 100755 --- a/tools/zookeeper/power_monitor.py +++ b/tools/zookeeper/power_monitor.py @@ -1,30 +1,30 @@ -#!/usr/bin/env python - +#!/usr/bin/env python3 import sys import time -from tools.zookeeper import Zookeeper -# Usage: check_consumption.py -# Exit code: 0 -> passed -# 1 -> failed +from common.realtime import Ratekeeper +from common.filter_simple import FirstOrderFilter +from tools.zookeeper import Zookeeper if __name__ == "__main__": z = Zookeeper() + z.set_device_power(True) + z.set_device_ignition(False) duration = None if len(sys.argv) > 1: duration = int(sys.argv[1]) + rate = 123 + rk = Ratekeeper(rate, print_delay_threshold=None) + fltr = FirstOrderFilter(0, 5, 1. / rate, initialized=False) + try: start_time = time.monotonic() - measurements = [] while duration is None or time.monotonic() - start_time < duration: - p = z.read_power() - print(round(p, 3), "W") - measurements.append(p) - time.sleep(0.25) + fltr.update(z.read_power()) + if rk.frame % rate == 0: + print(f"{fltr.x:.2f} W") + rk.keep_time() except KeyboardInterrupt: pass - finally: - average_power = sum(measurements)/len(measurements) - print(f"Average power: {round(average_power, 4)}W") diff --git a/tools/zookeeper/requirements.txt b/tools/zookeeper/requirements.txt deleted file mode 100644 index 6f70576caf..0000000000 --- a/tools/zookeeper/requirements.txt +++ /dev/null @@ -1 +0,0 @@ -ft4222==1.2.1 \ No newline at end of file From 0d0f5926a006ead1709b78fe69c1186b5a607c66 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Tue, 14 Jun 2022 21:12:49 -0700 Subject: [PATCH 77/93] bump up modeld power draw threshold --- system/hardware/tici/test_power_draw.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/hardware/tici/test_power_draw.py b/system/hardware/tici/test_power_draw.py index 31b8471328..b6b5c735fa 100755 --- a/system/hardware/tici/test_power_draw.py +++ b/system/hardware/tici/test_power_draw.py @@ -20,7 +20,7 @@ class Proc: PROCS = [ Proc('camerad', 2.15), - Proc('modeld', 1.0), + Proc('modeld', 1.0, atol=0.15), Proc('dmonitoringmodeld', 0.25), Proc('encoderd', 0.23), ] From a6652a539d8d4ba614b216900c472cc9e2e58a05 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Tue, 14 Jun 2022 22:29:08 -0700 Subject: [PATCH 78/93] Torque control: low speed boost (#24859) * Make very low speed more aggressive * Less extreme low speed boost * Update ref --- selfdrive/controls/lib/latcontrol_torque.py | 11 ++++++----- selfdrive/test/process_replay/ref_commit | 2 +- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 12714082a6..f72ffc4b88 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -19,8 +19,7 @@ from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY # move it at all, this is compensated for too. -LOW_SPEED_FACTOR = 200 -JERK_THRESHOLD = 0.2 +FRICTION_THRESHOLD = 0.2 def set_torque_tune(tune, MAX_LAT_ACCEL=2.5, FRICTION=0.01, steering_angle_deadzone_deg=0.0): @@ -66,14 +65,16 @@ class LatControlTorque(LatControl): actual_lateral_accel = actual_curvature * CS.vEgo ** 2 lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 - setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_curvature - measurement = actual_lateral_accel + LOW_SPEED_FACTOR * actual_curvature + + low_speed_factor = interp(CS.vEgo, [0, 15], [500, 0]) + setpoint = desired_lateral_accel + low_speed_factor * desired_curvature + measurement = actual_lateral_accel + low_speed_factor * actual_curvature error = apply_deadzone(setpoint - measurement, lateral_accel_deadzone) pid_log.error = error ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY # convert friction into lateral accel units for feedforward - friction_compensation = interp(error, [-JERK_THRESHOLD, JERK_THRESHOLD], [-self.friction, self.friction]) + friction_compensation = interp(error, [-FRICTION_THRESHOLD, FRICTION_THRESHOLD], [-self.friction, self.friction]) ff += friction_compensation / self.kf freeze_integrator = CS.steeringRateLimited or CS.steeringPressed or CS.vEgo < 5 output_torque = self.pid.update(error, diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 61bbd5cdd1..b8976e5f38 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -fe2da24194e3def1823681cc18e7879f24edfc6e +1d66eed104dbc124c4e5679f5dddf40197b86ce9 From b86ef0b70e81ef4e895d5102feb231dc3d43265f Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 14 Jun 2022 23:30:35 -0700 Subject: [PATCH 79/93] regen & process replay: support no disengage on accelerator (#24850) * ACC on if enabled != 0 * small regen clean up and add HONDA3 * fixes * revert unneeded changes * not used * just alt exp Co-authored-by: redacid95 --- selfdrive/test/process_replay/process_replay.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 363035ab22..b016eb1c9f 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -14,6 +14,7 @@ from cereal import car, log from cereal.services import service_list from common.params import Params from common.timeout import Timeout +from panda.python import ALTERNATIVE_EXPERIENCE from selfdrive.car.fingerprints import FW_VERSIONS from selfdrive.car.car_helpers import get_car, interfaces from selfdrive.test.process_replay.helpers import OpenpilotPrefix @@ -368,6 +369,9 @@ def setup_env(simulation=False, CP=None): # Regen or python process if CP is not None: + if CP.alternativeExperience == ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS: + params.put_bool("DisengageOnAccelerator", False) + if CP.fingerprintSource == "fw" and CP.carFingerprint in FW_VERSIONS: params.put("CarParamsCache", CP.as_builder().to_bytes()) else: From c3fa9151f39994984b60a19cdd7425dba73ec2fc Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Wed, 15 Jun 2022 11:32:07 +0200 Subject: [PATCH 80/93] Laikad: Cache orbit and nav data (#24831) * Cache orbit and nav data * Cleanup * Cleanup * Use ProcessPoolExecutor to fetch orbits * update laika repo * Minor * Create json de/serializers Save cache only 1 minute at max * Update laika repo * Speed up json by caching json in ephemeris class * Update laika * Fix test * Use constant --- common/params.cc | 1 + laika_repo | 2 +- selfdrive/locationd/laikad.py | 64 +++++++++++++++++++++--- selfdrive/locationd/test/test_laikad.py | 66 ++++++++++++++++++++++--- 4 files changed, 118 insertions(+), 15 deletions(-) diff --git a/common/params.cc b/common/params.cc index b740e5a71e..8cf1baa6c8 100644 --- a/common/params.cc +++ b/common/params.cc @@ -127,6 +127,7 @@ std::unordered_map keys = { {"IsTakingSnapshot", CLEAR_ON_MANAGER_START}, {"IsUpdateAvailable", CLEAR_ON_MANAGER_START}, {"JoystickDebugMode", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, + {"LaikadEphemeris", PERSISTENT}, {"LastAthenaPingTime", CLEAR_ON_MANAGER_START}, {"LastGPSPosition", PERSISTENT}, {"LastManagerExitReason", CLEAR_ON_MANAGER_START}, diff --git a/laika_repo b/laika_repo index 36f2621fc5..a3a80dc4f7 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit 36f2621fc5348487bb2cd606c37c8c15de0e32cd +Subproject commit a3a80dc4f7977b2232946e56a16770e413190818 diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 33e41398a0..ddd57bdca9 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +import json import time from concurrent.futures import Future, ProcessPoolExecutor from typing import List, Optional @@ -9,9 +10,10 @@ from collections import defaultdict from numpy.linalg import linalg from cereal import log, messaging +from common.params import Params, put_nonblocking from laika import AstroDog from laika.constants import SECS_IN_HR, SECS_IN_MIN -from laika.ephemeris import EphemerisType, convert_ublox_ephem +from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_ephem from laika.gps_time import GPSTime from laika.helpers import ConstellationId from laika.raw_gnss import GNSSMeasurement, calc_pos_fix, correct_measurements, process_measurements, read_raw_ublox @@ -22,16 +24,40 @@ import common.transformations.coordinates as coord from system.swaglog import cloudlog MAX_TIME_GAP = 10 +EPHEMERIS_CACHE = 'LaikadEphemeris' +CACHE_VERSION = 0.1 class Laikad: - - def __init__(self, valid_const=("GPS", "GLONASS"), auto_update=False, valid_ephem_types=(EphemerisType.ULTRA_RAPID_ORBIT, EphemerisType.NAV)): - self.astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types) + def __init__(self, valid_const=("GPS", "GLONASS"), auto_update=False, valid_ephem_types=(EphemerisType.ULTRA_RAPID_ORBIT, EphemerisType.NAV), + save_ephemeris=False): + self.astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types, clear_old_ephemeris=True) self.gnss_kf = GNSSKalman(GENERATED_DIR) self.orbit_fetch_executor = ProcessPoolExecutor() self.orbit_fetch_future: Optional[Future] = None self.last_fetch_orbits_t = None + self.last_cached_t = None + self.save_ephemeris = save_ephemeris + self.load_cache() + + def load_cache(self): + cache = Params().get(EPHEMERIS_CACHE) + if not cache: + return + try: + cache = json.loads(cache, object_hook=deserialize_hook) + self.astro_dog.add_orbits(cache['orbits']) + self.astro_dog.add_navs(cache['nav']) + self.last_fetch_orbits_t = cache['last_fetch_orbits_t'] + except json.decoder.JSONDecodeError: + cloudlog.exception("Error parsing cache") + + def cache_ephemeris(self, t: GPSTime): + if self.save_ephemeris and (self.last_cached_t is None or t - self.last_cached_t > SECS_IN_MIN): + put_nonblocking(EPHEMERIS_CACHE, json.dumps( + {'version': CACHE_VERSION, 'last_fetch_orbits_t': self.last_fetch_orbits_t, 'orbits': self.astro_dog.orbits, 'nav': self.astro_dog.nav}, + cls=CacheSerializer)) + self.last_cached_t = t def process_ublox_msg(self, ublox_msg, ublox_mono_time: int, block=False): if ublox_msg.which == 'measurementReport': @@ -83,7 +109,8 @@ class Laikad: return dat elif ublox_msg.which == 'ephemeris': ephem = convert_ublox_ephem(ublox_msg.ephemeris) - self.astro_dog.add_navs([ephem]) + self.astro_dog.add_navs({ephem.prn: [ephem]}) + self.cache_ephemeris(t=ephem.epoch) # elif ublox_msg.which == 'ionoData': # todo add this. Needed to better correct messages offline. First fix ublox_msg.cc to sent them. @@ -101,7 +128,7 @@ class Laikad: cloudlog.error("Gnss kalman std too far") if len(pos_fix) == 0: - cloudlog.warning("Position fix not available when resetting kalman filter") + cloudlog.info("Position fix not available when resetting kalman filter") return post_est = pos_fix[0][:3].tolist() self.init_gnss_localizer(post_est) @@ -134,10 +161,11 @@ class Laikad: self.orbit_fetch_future.result() if self.orbit_fetch_future.done(): ret = self.orbit_fetch_future.result() + self.last_fetch_orbits_t = t if ret: self.astro_dog.orbits, self.astro_dog.orbit_fetched_times = ret + self.cache_ephemeris(t=t) self.orbit_fetch_future = None - self.last_fetch_orbits_t = t def get_orbit_data(t: GPSTime, valid_const, auto_update, valid_ephem_types): @@ -193,11 +221,31 @@ def get_bearing_from_gnss(ecef_pos, ecef_vel, vel_std): return float(np.rad2deg(bearing)), float(bearing_std) +class CacheSerializer(json.JSONEncoder): + + def default(self, o): + if isinstance(o, Ephemeris): + return o.to_json() + if isinstance(o, GPSTime): + return o.__dict__ + if isinstance(o, np.ndarray): + return o.tolist() + return json.JSONEncoder.default(self, o) + + +def deserialize_hook(dct): + if 'ephemeris' in dct: + return Ephemeris.from_json(dct) + if 'week' in dct: + return GPSTime(dct['week'], dct['tow']) + return dct + + def main(): sm = messaging.SubMaster(['ubloxGnss']) pm = messaging.PubMaster(['gnssMeasurements']) - laikad = Laikad() + laikad = Laikad(save_ephemeris=True) while True: sm.update() diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py index 7dc803d799..01ea8fee27 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/selfdrive/locationd/test/test_laikad.py @@ -1,14 +1,16 @@ #!/usr/bin/env python3 +import time import unittest from datetime import datetime from unittest import mock -from unittest.mock import Mock +from unittest.mock import Mock, patch +from common.params import Params from laika.ephemeris import EphemerisType from laika.gps_time import GPSTime -from laika.helpers import ConstellationId +from laika.helpers import ConstellationId, TimeRangeHolder from laika.raw_gnss import GNSSMeasurement, read_raw_ublox -from selfdrive.locationd.laikad import Laikad, create_measurement_msg +from selfdrive.locationd.laikad import EPHEMERIS_CACHE, Laikad, create_measurement_msg from selfdrive.test.openpilotci import get_url from tools.lib.logreader import LogReader @@ -20,12 +22,14 @@ def get_log(segs=range(0)): return [m for m in logs if m.which() == 'ubloxGnss'] -def verify_messages(lr, laikad): +def verify_messages(lr, laikad, return_one_success=False): good_msgs = [] for m in lr: msg = laikad.process_ublox_msg(m.ubloxGnss, m.logMonoTime, block=True) if msg is not None and len(msg.gnssMeasurements.correctedMeasurements) > 0: good_msgs.append(msg) + if return_one_success: + return msg return good_msgs @@ -35,6 +39,9 @@ class TestLaikad(unittest.TestCase): def setUpClass(cls): cls.logs = get_log(range(1)) + def setUp(self): + Params().delete(EPHEMERIS_CACHE) + def test_create_msg_without_errors(self): gpstime = GPSTime.from_datetime(datetime.now()) meas = GNSSMeasurement(ConstellationId.GPS, 1, gpstime.week, gpstime.tow, {'C1C': 0., 'D1C': 0.}, {'C1C': 0., 'D1C': 0.}) @@ -81,8 +88,7 @@ class TestLaikad(unittest.TestCase): first_gps_time = self.get_first_gps_time() # Pretend process has loaded the orbits on startup by using the time of the first gps message. laikad.fetch_orbits(first_gps_time, block=True) - self.assertEqual(29, len(laikad.astro_dog.orbits.values())) - self.assertGreater(min([len(v) for v in laikad.astro_dog.orbits.values()]), 0) + self.dict_has_values(laikad.astro_dog.orbits) @unittest.skip("Use to debug live data") def test_laika_get_orbits_now(self): @@ -109,6 +115,54 @@ class TestLaikad(unittest.TestCase): self.assertGreater(len(laikad.astro_dog.orbit_fetched_times._ranges), 0) self.assertEqual(None, laikad.orbit_fetch_future) + def test_cache(self): + laikad = Laikad(auto_update=True, save_ephemeris=True) + first_gps_time = self.get_first_gps_time() + + def wait_for_cache(): + max_time = 2 + while Params().get(EPHEMERIS_CACHE) is None: + time.sleep(0.1) + max_time -= 0.1 + if max_time == 0: + self.fail("Cache has not been written after 2 seconds") + # Test cache with no ephemeris + laikad.cache_ephemeris(t=GPSTime(0, 0)) + wait_for_cache() + Params().delete(EPHEMERIS_CACHE) + + laikad.astro_dog.get_navs(first_gps_time) + laikad.fetch_orbits(first_gps_time, block=True) + + # Wait for cache to save + wait_for_cache() + + # Check both nav and orbits separate + laikad = Laikad(auto_update=False, valid_ephem_types=EphemerisType.NAV) + # Verify orbits and nav are loaded from cache + self.dict_has_values(laikad.astro_dog.orbits) + self.dict_has_values(laikad.astro_dog.nav) + # Verify cache is working for only nav by running a segment + msg = verify_messages(self.logs, laikad, return_one_success=True) + self.assertIsNotNone(msg) + + with patch('selfdrive.locationd.laikad.get_orbit_data', return_value=None) as mock_method: + # Verify no orbit downloads even if orbit fetch times is reset since the cache has recently been saved and we don't want to download high frequently + laikad.astro_dog.orbit_fetched_times = TimeRangeHolder() + laikad.fetch_orbits(first_gps_time, block=False) + mock_method.assert_not_called() + + # Verify cache is working for only orbits by running a segment + laikad = Laikad(auto_update=False, valid_ephem_types=EphemerisType.ULTRA_RAPID_ORBIT) + msg = verify_messages(self.logs, laikad, return_one_success=True) + self.assertIsNotNone(msg) + # Verify orbit data is not downloaded + mock_method.assert_not_called() + + def dict_has_values(self, dct): + self.assertGreater(len(dct), 0) + self.assertGreater(min([len(v) for v in dct.values()]), 0) + if __name__ == "__main__": unittest.main() From d528cd556848404891e28b3342d146498e3991cc Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 15 Jun 2022 13:06:46 +0200 Subject: [PATCH 81/93] UI: new set speed design, show speed limits (#24736) * basic US design * place based on center position * fix typo * eu sign without rounded box * same as steering wheel icon * proper rounded bottom for eu sign * add border * proper placement/sizes * needs to be semi bold * color changes * only when engaged * move helpers into util.h * Fix MAX placement * only change color when at least 5 over * implement override state * pixel perfect spacing around us sign --- selfdrive/ui/qt/onroad.cc | 162 ++++++++++++++++++++++++++++++++------ selfdrive/ui/qt/onroad.h | 15 +++- selfdrive/ui/qt/util.cc | 61 ++++++++++++++ selfdrive/ui/qt/util.h | 4 + 4 files changed, 216 insertions(+), 26 deletions(-) diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index ce61c094bd..12a579c573 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -174,17 +174,24 @@ void NvgWindow::updateState(const UIState &s) { const SubMaster &sm = *(s.sm); const auto cs = sm["controlsState"].getControlsState(); - float maxspeed = cs.getVCruise(); - bool cruise_set = maxspeed > 0 && (int)maxspeed != SET_SPEED_NA; + float set_speed = cs.getVCruise(); + bool cruise_set = set_speed > 0 && (int)set_speed != SET_SPEED_NA; if (cruise_set && !s.scene.is_metric) { - maxspeed *= KM_TO_MILE; + set_speed *= KM_TO_MILE; } - QString maxspeed_str = cruise_set ? QString::number(std::nearbyint(maxspeed)) : "N/A"; float cur_speed = std::max(0.0, sm["carState"].getCarState().getVEgo() * (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH)); + auto speed_limit_sign = sm["navInstruction"].getNavInstruction().getSpeedLimitSign(); + float speed_limit = sm["navInstruction"].getValid() ? sm["navInstruction"].getNavInstruction().getSpeedLimit() : 0.0; + speed_limit *= (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH); + + setProperty("speedLimit", speed_limit); + setProperty("has_us_speed_limit", speed_limit > 1 && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD); + setProperty("has_eu_speed_limit", speed_limit > 1 && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA); + setProperty("is_cruise_set", cruise_set); - setProperty("speed", QString::number(std::nearbyint(cur_speed))); - setProperty("maxSpeed", maxspeed_str); + setProperty("speed", cur_speed); + setProperty("setSpeed", set_speed); setProperty("speedUnit", s.scene.is_metric ? "km/h" : "mph"); setProperty("hideDM", cs.getAlertSize() != cereal::ControlsState::AlertSize::NONE); setProperty("status", s.status); @@ -205,26 +212,139 @@ void NvgWindow::drawHud(QPainter &p) { bg.setColorAt(1, QColor::fromRgbF(0, 0, 0, 0)); p.fillRect(0, 0, width(), header_h, bg); - // max speed - QRect rc(bdr_s * 2, bdr_s * 1.5, 184, 202); - p.setPen(QPen(QColor(0xff, 0xff, 0xff, 100), 10)); - p.setBrush(QColor(0, 0, 0, 100)); - p.drawRoundedRect(rc, 20, 20); - p.setPen(Qt::NoPen); + QString speedLimitStr = QString::number(std::nearbyint(speedLimit)); + QString speedStr = QString::number(std::nearbyint(speed)); + QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed)) : "–"; + + // Draw outer box + border to contain set speed and speed limit + int default_rect_width = 172; + int rect_width = default_rect_width; + if (has_us_speed_limit && speedLimitStr.size() >= 3) rect_width = 223; + else if (has_eu_speed_limit) rect_width = 200; + + int rect_height = 204; + if (has_us_speed_limit) rect_height = 402; + else if (has_eu_speed_limit) rect_height = 392; - configFont(p, "Open Sans", 48, "Regular"); - drawText(p, rc.center().x(), 118, "MAX", is_cruise_set ? 200 : 100); + int top_radius = 32; + int bottom_radius = has_eu_speed_limit ? 100 : 32; + + QRect set_speed_rect(60 + default_rect_width / 2 - rect_width / 2, 45, rect_width, rect_height); + p.setPen(QPen(QColor(255, 255, 255, 75), 6)); + p.setBrush(QColor(0, 0, 0, 166)); + drawRoundedRect(p, set_speed_rect, top_radius, top_radius, bottom_radius, bottom_radius); + + // Draw set speed if (is_cruise_set) { - configFont(p, "Open Sans", 88, "Bold"); - drawText(p, rc.center().x(), 212, maxSpeed, 255); + if (speedLimit > 0 && status != STATUS_DISENGAGED && status != STATUS_OVERRIDE) { + p.setPen(interpColor( + setSpeed, + {speedLimit + 5, speedLimit + 15, speedLimit + 25}, + {QColor(0xff, 0xff, 0xff, 0xff), QColor(0xff, 0x95, 0x00, 0xff), QColor(0xff, 0x00, 0x00, 0xff)} + )); + } else { + p.setPen(QColor(0xff, 0xff, 0xff, 0xff)); + } } else { - configFont(p, "Open Sans", 80, "SemiBold"); - drawText(p, rc.center().x(), 212, maxSpeed, 100); + p.setPen(QColor(0x72, 0x72, 0x72, 0xff)); + } + configFont(p, "Open Sans", 90, "Bold"); + QRect speed_rect = getTextRect(p, Qt::AlignCenter, setSpeedStr); + speed_rect.moveCenter({set_speed_rect.center().x(), 0}); + speed_rect.moveTop(set_speed_rect.top() + 8); + p.drawText(speed_rect, Qt::AlignCenter, setSpeedStr); + + // Draw MAX + if (is_cruise_set) { + if (status == STATUS_DISENGAGED) { + p.setPen(QColor(0xff, 0xff, 0xff, 0xff)); + } else if (status == STATUS_OVERRIDE) { + p.setPen(QColor(0x91, 0x9b, 0x95, 0xff)); + } else if (speedLimit > 0) { + p.setPen(interpColor( + setSpeed, + {speedLimit + 5, speedLimit + 15, speedLimit + 25}, + {QColor(0x80, 0xd8, 0xa6, 0xff), QColor(0xff, 0xe4, 0xbf, 0xff), QColor(0xff, 0xbf, 0xbf, 0xff)} + )); + } else { + p.setPen(QColor(0x80, 0xd8, 0xa6, 0xff)); + } + } else { + p.setPen(QColor(0xa6, 0xa6, 0xa6, 0xff)); + } + configFont(p, "Open Sans", 40, "SemiBold"); + QRect max_rect = getTextRect(p, Qt::AlignCenter, "MAX"); + max_rect.moveCenter({set_speed_rect.center().x(), 0}); + max_rect.moveTop(set_speed_rect.top() + 123); + p.drawText(max_rect, Qt::AlignCenter, "MAX"); + + // US/Canada (MUTCD style) sign + if (has_us_speed_limit) { + const int border_width = 6; + const int sign_width = (speedLimitStr.size() >= 3) ? 199 : 148; + const int sign_height = 186; + + // White outer square + QRect sign_rect_outer(set_speed_rect.left() + 12, set_speed_rect.bottom() - 11 - sign_height, sign_width, sign_height); + p.setPen(Qt::NoPen); + p.setBrush(QColor(255, 255, 255, 255)); + p.drawRoundedRect(sign_rect_outer, 24, 24); + + // Smaller white square with black border + QRect sign_rect(sign_rect_outer.left() + 1.5 * border_width, sign_rect_outer.top() + 1.5 * border_width, sign_width - 3 * border_width, sign_height - 3 * border_width); + p.setPen(QPen(QColor(0, 0, 0, 255), border_width)); + p.setBrush(QColor(255, 255, 255, 255)); + p.drawRoundedRect(sign_rect, 16, 16); + + // "SPEED" + configFont(p, "Open Sans", 28, "SemiBold"); + QRect text_speed_rect = getTextRect(p, Qt::AlignCenter, "SPEED"); + text_speed_rect.moveCenter({sign_rect.center().x(), 0}); + text_speed_rect.moveTop(sign_rect_outer.top() + 20); + p.drawText(text_speed_rect, Qt::AlignCenter, "SPEED"); + + // "LIMIT" + QRect text_limit_rect = getTextRect(p, Qt::AlignCenter, "LIMIT"); + text_limit_rect.moveCenter({sign_rect.center().x(), 0}); + text_limit_rect.moveTop(sign_rect_outer.top() + 48); + p.drawText(text_limit_rect, Qt::AlignCenter, "LIMIT"); + + // Speed limit value + configFont(p, "Open Sans", 70, "Bold"); + QRect speed_limit_rect = getTextRect(p, Qt::AlignCenter, speedLimitStr); + speed_limit_rect.moveCenter({sign_rect.center().x(), 0}); + speed_limit_rect.moveTop(sign_rect.top() + 70); + p.drawText(speed_limit_rect, Qt::AlignCenter, speedLimitStr); + } + + // EU (Vienna style) sign + if (has_eu_speed_limit) { + int outer_radius = 176 / 2; + int inner_radius_1 = outer_radius - 6; // White outer border + int inner_radius_2 = inner_radius_1 - 20; // Red circle + + // Draw white circle with red border + QPoint center(set_speed_rect.center().x() + 1, set_speed_rect.top() + 204 + outer_radius); + p.setPen(Qt::NoPen); + p.setBrush(QColor(255, 255, 255, 255)); + p.drawEllipse(center, outer_radius, outer_radius); + p.setBrush(QColor(255, 0, 0, 255)); + p.drawEllipse(center, inner_radius_1, inner_radius_1); + p.setBrush(QColor(255, 255, 255, 255)); + p.drawEllipse(center, inner_radius_2, inner_radius_2); + + // Speed limit value + int font_size = (speedLimitStr.size() >= 3) ? 62 : 70; + configFont(p, "Open Sans", font_size, "Bold"); + QRect speed_limit_rect = getTextRect(p, Qt::AlignCenter, speedLimitStr); + speed_limit_rect.moveCenter(center); + p.setPen(QColor(0, 0, 0, 255)); + p.drawText(speed_limit_rect, Qt::AlignCenter, speedLimitStr); } // current speed configFont(p, "Open Sans", 176, "Bold"); - drawText(p, rect().center().x(), 210, speed); + drawText(p, rect().center().x(), 210, speedStr); configFont(p, "Open Sans", 66, "Regular"); drawText(p, rect().center().x(), 290, speedUnit, 200); @@ -243,9 +363,7 @@ void NvgWindow::drawHud(QPainter &p) { } void NvgWindow::drawText(QPainter &p, int x, int y, const QString &text, int alpha) { - QFontMetrics fm(p.font()); - QRect init_rect = fm.boundingRect(text); - QRect real_rect = fm.boundingRect(init_rect, 0, text); + QRect real_rect = getTextRect(p, 0, text); real_rect.moveCenter({x, y - real_rect.height() / 2}); p.setPen(QColor(0xff, 0xff, 0xff, alpha)); diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 6ca2b3c738..3d90a63d7c 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -27,10 +27,14 @@ private: // container window for the NVG UI class NvgWindow : public CameraViewWidget { Q_OBJECT - Q_PROPERTY(QString speed MEMBER speed); + Q_PROPERTY(float speed MEMBER speed); Q_PROPERTY(QString speedUnit MEMBER speedUnit); - Q_PROPERTY(QString maxSpeed MEMBER maxSpeed); + Q_PROPERTY(float setSpeed MEMBER setSpeed); + Q_PROPERTY(float speedLimit MEMBER speedLimit); Q_PROPERTY(bool is_cruise_set MEMBER is_cruise_set); + Q_PROPERTY(bool has_eu_speed_limit MEMBER has_eu_speed_limit); + Q_PROPERTY(bool has_us_speed_limit MEMBER has_us_speed_limit); + Q_PROPERTY(bool engageable MEMBER engageable); Q_PROPERTY(bool dmActive MEMBER dmActive); Q_PROPERTY(bool hideDM MEMBER hideDM); @@ -48,13 +52,16 @@ private: QPixmap dm_img; const int radius = 192; const int img_size = (radius / 2) * 1.5; - QString speed; + float speed; QString speedUnit; - QString maxSpeed; + float setSpeed; + float speedLimit; bool is_cruise_set = false; bool engageable = false; bool dmActive = false; bool hideDM = false; + bool has_us_speed_limit = false; + bool has_eu_speed_limit = false; int status = STATUS_DISENGAGED; protected: diff --git a/selfdrive/ui/qt/util.cc b/selfdrive/ui/qt/util.cc index 600885fb37..408652a0b9 100644 --- a/selfdrive/ui/qt/util.cc +++ b/selfdrive/ui/qt/util.cc @@ -135,3 +135,64 @@ QPixmap loadPixmap(const QString &fileName, const QSize &size, Qt::AspectRatioMo return QPixmap(fileName).scaled(size, aspectRatioMode, Qt::SmoothTransformation); } } + +QRect getTextRect(QPainter &p, int flags, QString text) { + QFontMetrics fm(p.font()); + QRect init_rect = fm.boundingRect(text); + return fm.boundingRect(init_rect, flags, text); +} + +void drawRoundedRect(QPainter &painter, const QRectF &rect, qreal xRadiusTop, qreal yRadiusTop, qreal xRadiusBottom, qreal yRadiusBottom){ + qreal w_2 = rect.width() / 2; + qreal h_2 = rect.height() / 2; + + xRadiusTop = 100 * qMin(xRadiusTop, w_2) / w_2; + yRadiusTop = 100 * qMin(yRadiusTop, h_2) / h_2; + + xRadiusBottom = 100 * qMin(xRadiusBottom, w_2) / w_2; + yRadiusBottom = 100 * qMin(yRadiusBottom, h_2) / h_2; + + qreal x = rect.x(); + qreal y = rect.y(); + qreal w = rect.width(); + qreal h = rect.height(); + + qreal rxx2Top = w*xRadiusTop/100; + qreal ryy2Top = h*yRadiusTop/100; + + qreal rxx2Bottom = w*xRadiusBottom/100; + qreal ryy2Bottom = h*yRadiusBottom/100; + + QPainterPath path; + path.arcMoveTo(x, y, rxx2Top, ryy2Top, 180); + path.arcTo(x, y, rxx2Top, ryy2Top, 180, -90); + path.arcTo(x+w-rxx2Top, y, rxx2Top, ryy2Top, 90, -90); + path.arcTo(x+w-rxx2Bottom, y+h-ryy2Bottom, rxx2Bottom, ryy2Bottom, 0, -90); + path.arcTo(x, y+h-ryy2Bottom, rxx2Bottom, ryy2Bottom, 270, -90); + path.closeSubpath(); + + painter.drawPath(path); +} + +QColor interpColor(float xv, std::vector xp, std::vector fp) { + assert(xp.size() == fp.size()); + + int N = xp.size(); + int hi = 0; + + while (hi < N and xv > xp[hi]) hi++; + int low = hi - 1; + + if (hi == N && xv > xp[low]) { + return fp[fp.size() - 1]; + } else if (hi == 0){ + return fp[0]; + } else { + return QColor( + (xv - xp[low]) * (fp[hi].red() - fp[low].red()) / (xp[hi] - xp[low]) + fp[low].red(), + (xv - xp[low]) * (fp[hi].green() - fp[low].green()) / (xp[hi] - xp[low]) + fp[low].green(), + (xv - xp[low]) * (fp[hi].blue() - fp[low].blue()) / (xp[hi] - xp[low]) + fp[low].blue(), + (xv - xp[low]) * (fp[hi].alpha() - fp[low].alpha()) / (xp[hi] - xp[low]) + fp[low].alpha() + ); + } +} diff --git a/selfdrive/ui/qt/util.h b/selfdrive/ui/qt/util.h index 813777710a..9491c6798e 100644 --- a/selfdrive/ui/qt/util.h +++ b/selfdrive/ui/qt/util.h @@ -22,3 +22,7 @@ void swagLogMessageHandler(QtMsgType type, const QMessageLogContext &context, co void initApp(int argc, char *argv[]); QWidget* topWidget (QWidget* widget); QPixmap loadPixmap(const QString &fileName, const QSize &size = {}, Qt::AspectRatioMode aspectRatioMode = Qt::KeepAspectRatio); + +QRect getTextRect(QPainter &p, int flags, QString text); +void drawRoundedRect(QPainter &painter, const QRectF &rect, qreal xRadiusTop, qreal yRadiusTop, qreal xRadiusBottom, qreal yRadiusBottom); +QColor interpColor(float xv, std::vector xp, std::vector fp); From b046eb818e35f24d3b9b8a978fddb94dbfb85e59 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 15 Jun 2022 13:09:57 +0200 Subject: [PATCH 82/93] add speed limits to release notes --- RELEASES.md | 1 + 1 file changed, 1 insertion(+) diff --git a/RELEASES.md b/RELEASES.md index acbafd5f5b..7f365e5cef 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -10,6 +10,7 @@ Version 0.8.15 (2022-XX-XX) * takes a larger input frame * outputs a driver state for both driver and passenger * automatically determines which side the driver is on (soon) +* Display speed limit while navigating * Reduced power usage: device runs cooler and fan spins less * AGNOS 5 * Hyundai Tucson 2021 support thanks to bluesforte! From 9e3d0e3c9cea4dcf2da10801aad041b1dd65cccb Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Wed, 15 Jun 2022 14:00:27 +0200 Subject: [PATCH 83/93] Laikad: Remove bearingDeg from message (#24864) * Remove bearingDeg from message. * Push cereal * Commit cereal --- cereal | 2 +- selfdrive/locationd/laikad.py | 14 -------------- 2 files changed, 1 insertion(+), 15 deletions(-) diff --git a/cereal b/cereal index 98f795fd73..4b4d9aab03 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 98f795fd73356198f1fc8d5ea5a8f25e6f7c57e0 +Subproject commit 4b4d9aab03937f5a45677ff15efb275abf9c958b diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index ddd57bdca9..ebe7404e17 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -20,7 +20,6 @@ from laika.raw_gnss import GNSSMeasurement, calc_pos_fix, correct_measurements, from selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind from selfdrive.locationd.models.gnss_kf import GNSSKalman from selfdrive.locationd.models.gnss_kf import States as GStates -import common.transformations.coordinates as coord from system.swaglog import cloudlog MAX_TIME_GAP = 10 @@ -94,15 +93,12 @@ class Laikad: pos_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_POS].diagonal())).tolist() vel_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_VELOCITY].diagonal())).tolist() - bearing_deg, bearing_std = get_bearing_from_gnss(ecef_pos, ecef_vel, vel_std) - meas_msgs = [create_measurement_msg(m) for m in corrected_measurements] dat = messaging.new_message("gnssMeasurements") measurement_msg = log.LiveLocationKalman.Measurement.new_message dat.gnssMeasurements = { "positionECEF": measurement_msg(value=ecef_pos, std=pos_std, valid=kf_valid), "velocityECEF": measurement_msg(value=ecef_vel, std=vel_std, valid=kf_valid), - "bearingDeg": measurement_msg(value=[bearing_deg], std=[bearing_std], valid=kf_valid), "ubloxMonoTime": ublox_mono_time, "correctedMeasurements": meas_msgs } @@ -211,16 +207,6 @@ def kf_add_observations(gnss_kf: GNSSKalman, t: float, measurements: List[GNSSMe gnss_kf.predict_and_observe(t, kind, data) -def get_bearing_from_gnss(ecef_pos, ecef_vel, vel_std): - # init orientation with direction of velocity - converter = coord.LocalCoord.from_ecef(ecef_pos) - - ned_vel = np.einsum('ij,j ->i', converter.ned_from_ecef_matrix, ecef_vel) - bearing = np.arctan2(ned_vel[1], ned_vel[0]) - bearing_std = np.arctan2(np.linalg.norm(vel_std), np.linalg.norm(ned_vel)) - return float(np.rad2deg(bearing)), float(bearing_std) - - class CacheSerializer(json.JSONEncoder): def default(self, o): From 849ec17b20f8cae7f6fd6589456d7b00b07e794b Mon Sep 17 00:00:00 2001 From: Lee Jong Mun <43285072+crwusiz@users.noreply.github.com> Date: Wed, 15 Jun 2022 23:10:56 +0900 Subject: [PATCH 84/93] ui: add black color define (#24866) --- selfdrive/ui/qt/onroad.cc | 24 ++++++++++++------------ selfdrive/ui/qt/onroad.h | 1 + 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 12a579c573..38541ef2d5 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -230,8 +230,8 @@ void NvgWindow::drawHud(QPainter &p) { int bottom_radius = has_eu_speed_limit ? 100 : 32; QRect set_speed_rect(60 + default_rect_width / 2 - rect_width / 2, 45, rect_width, rect_height); - p.setPen(QPen(QColor(255, 255, 255, 75), 6)); - p.setBrush(QColor(0, 0, 0, 166)); + p.setPen(QPen(whiteColor(75), 6)); + p.setBrush(blackColor(166)); drawRoundedRect(p, set_speed_rect, top_radius, top_radius, bottom_radius, bottom_radius); // Draw set speed @@ -240,10 +240,10 @@ void NvgWindow::drawHud(QPainter &p) { p.setPen(interpColor( setSpeed, {speedLimit + 5, speedLimit + 15, speedLimit + 25}, - {QColor(0xff, 0xff, 0xff, 0xff), QColor(0xff, 0x95, 0x00, 0xff), QColor(0xff, 0x00, 0x00, 0xff)} + {whiteColor(), QColor(0xff, 0x95, 0x00, 0xff), QColor(0xff, 0x00, 0x00, 0xff)} )); } else { - p.setPen(QColor(0xff, 0xff, 0xff, 0xff)); + p.setPen(whiteColor()); } } else { p.setPen(QColor(0x72, 0x72, 0x72, 0xff)); @@ -257,7 +257,7 @@ void NvgWindow::drawHud(QPainter &p) { // Draw MAX if (is_cruise_set) { if (status == STATUS_DISENGAGED) { - p.setPen(QColor(0xff, 0xff, 0xff, 0xff)); + p.setPen(whiteColor()); } else if (status == STATUS_OVERRIDE) { p.setPen(QColor(0x91, 0x9b, 0x95, 0xff)); } else if (speedLimit > 0) { @@ -287,13 +287,13 @@ void NvgWindow::drawHud(QPainter &p) { // White outer square QRect sign_rect_outer(set_speed_rect.left() + 12, set_speed_rect.bottom() - 11 - sign_height, sign_width, sign_height); p.setPen(Qt::NoPen); - p.setBrush(QColor(255, 255, 255, 255)); + p.setBrush(whiteColor()); p.drawRoundedRect(sign_rect_outer, 24, 24); // Smaller white square with black border QRect sign_rect(sign_rect_outer.left() + 1.5 * border_width, sign_rect_outer.top() + 1.5 * border_width, sign_width - 3 * border_width, sign_height - 3 * border_width); - p.setPen(QPen(QColor(0, 0, 0, 255), border_width)); - p.setBrush(QColor(255, 255, 255, 255)); + p.setPen(QPen(blackColor(), border_width)); + p.setBrush(whiteColor()); p.drawRoundedRect(sign_rect, 16, 16); // "SPEED" @@ -326,11 +326,11 @@ void NvgWindow::drawHud(QPainter &p) { // Draw white circle with red border QPoint center(set_speed_rect.center().x() + 1, set_speed_rect.top() + 204 + outer_radius); p.setPen(Qt::NoPen); - p.setBrush(QColor(255, 255, 255, 255)); + p.setBrush(whiteColor()); p.drawEllipse(center, outer_radius, outer_radius); p.setBrush(QColor(255, 0, 0, 255)); p.drawEllipse(center, inner_radius_1, inner_radius_1); - p.setBrush(QColor(255, 255, 255, 255)); + p.setBrush(whiteColor()); p.drawEllipse(center, inner_radius_2, inner_radius_2); // Speed limit value @@ -338,7 +338,7 @@ void NvgWindow::drawHud(QPainter &p) { configFont(p, "Open Sans", font_size, "Bold"); QRect speed_limit_rect = getTextRect(p, Qt::AlignCenter, speedLimitStr); speed_limit_rect.moveCenter(center); - p.setPen(QColor(0, 0, 0, 255)); + p.setPen(blackColor()); p.drawText(speed_limit_rect, Qt::AlignCenter, speedLimitStr); } @@ -357,7 +357,7 @@ void NvgWindow::drawHud(QPainter &p) { // dm icon if (!hideDM) { drawIcon(p, radius / 2 + (bdr_s * 2), rect().bottom() - footer_h / 2, - dm_img, QColor(0, 0, 0, 70), dmActive ? 1.0 : 0.2); + dm_img, blackColor(70), dmActive ? 1.0 : 0.2); } p.restore(); } diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 3d90a63d7c..e58d7a7972 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -74,6 +74,7 @@ protected: void drawHud(QPainter &p); inline QColor redColor(int alpha = 255) { return QColor(201, 34, 49, alpha); } inline QColor whiteColor(int alpha = 255) { return QColor(255, 255, 255, alpha); } + inline QColor blackColor(int alpha = 255) { return QColor(0, 0, 0, alpha); } double prev_draw_t = 0; FirstOrderFilter fps_filter; From d2400150e51833678f8c216c0c605111cfa13018 Mon Sep 17 00:00:00 2001 From: Lee Jong Mun <43285072+crwusiz@users.noreply.github.com> Date: Wed, 15 Jun 2022 23:16:18 +0900 Subject: [PATCH 85/93] ui: advanced network settings fix button colors and sizes (#24846) * ui: button pressed color add * match colors from other buttons Co-authored-by: Willem Melching --- selfdrive/ui/qt/offroad/networking.cc | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/selfdrive/ui/qt/offroad/networking.cc b/selfdrive/ui/qt/offroad/networking.cc index 9c2088b1d7..80c7ad045b 100644 --- a/selfdrive/ui/qt/offroad/networking.cc +++ b/selfdrive/ui/qt/offroad/networking.cc @@ -28,9 +28,9 @@ Networking::Networking(QWidget* parent, bool show_advanced) : QFrame(parent) { vlayout->setContentsMargins(20, 20, 20, 20); if (show_advanced) { QPushButton* advancedSettings = new QPushButton("Advanced"); - advancedSettings->setObjectName("advancedBtn"); + advancedSettings->setObjectName("advanced_btn"); advancedSettings->setStyleSheet("margin-right: 30px;"); - advancedSettings->setFixedSize(350, 100); + advancedSettings->setFixedSize(400, 100); connect(advancedSettings, &QPushButton::clicked, [=]() { main_layout->setCurrentWidget(an); }); vlayout->addSpacing(10); vlayout->addWidget(advancedSettings, 0, Qt::AlignRight); @@ -57,14 +57,17 @@ Networking::Networking(QWidget* parent, bool show_advanced) : QFrame(parent) { // TODO: revisit pressed colors setStyleSheet(R"( - #wifiWidget > QPushButton, #back_btn, #advancedBtn { + #wifiWidget > QPushButton, #back_btn, #advanced_btn { font-size: 50px; margin: 0px; padding: 15px; border-width: 0; border-radius: 30px; color: #dddddd; - background-color: #444444; + background-color: #393939; + } + #back_btn:pressed, #advanced_btn:pressed { + background-color: #4a4a4a; } )"); main_layout->setCurrentWidget(wifiScreen); @@ -118,7 +121,7 @@ AdvancedNetworking::AdvancedNetworking(QWidget* parent, WifiManager* wifi): QWid // Back button QPushButton* back = new QPushButton("Back"); back->setObjectName("back_btn"); - back->setFixedSize(500, 100); + back->setFixedSize(400, 100); connect(back, &QPushButton::clicked, [=]() { emit backPress(); }); main_layout->addWidget(back, 0, Qt::AlignLeft); From bb02c1c5158b3403e7d2395611d2db1ab44aed5e Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 15 Jun 2022 16:19:04 +0200 Subject: [PATCH 86/93] networking.cc: remove resolved TODO --- selfdrive/ui/qt/offroad/networking.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/selfdrive/ui/qt/offroad/networking.cc b/selfdrive/ui/qt/offroad/networking.cc index 80c7ad045b..418ccd3179 100644 --- a/selfdrive/ui/qt/offroad/networking.cc +++ b/selfdrive/ui/qt/offroad/networking.cc @@ -55,7 +55,6 @@ Networking::Networking(QWidget* parent, bool show_advanced) : QFrame(parent) { setAutoFillBackground(true); setPalette(pal); - // TODO: revisit pressed colors setStyleSheet(R"( #wifiWidget > QPushButton, #back_btn, #advanced_btn { font-size: 50px; From 41721917239e8eac314a2dc6c64cfb4695ff460e Mon Sep 17 00:00:00 2001 From: Lee Jong Mun <43285072+crwusiz@users.noreply.github.com> Date: Thu, 16 Jun 2022 00:35:46 +0900 Subject: [PATCH 87/93] selfdrive/ui/qt/util.cc: missing include (#24867) --- selfdrive/ui/qt/util.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/ui/qt/util.cc b/selfdrive/ui/qt/util.cc index 408652a0b9..366aa76f49 100644 --- a/selfdrive/ui/qt/util.cc +++ b/selfdrive/ui/qt/util.cc @@ -3,6 +3,7 @@ #include #include #include +#include #include "common/params.h" #include "common/swaglog.h" From fa4f017bbe6c117913ec2327e9129c70b3e47223 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 15 Jun 2022 18:03:15 +0200 Subject: [PATCH 88/93] laikad: calc_pos_fix numpy implementation (#24865) * Replace posfix with gauss newton method * Cleanup * Check if glonass is in the list * Fix * also return residual * Add residuals * Update selfdrive/locationd/laikad.py Co-authored-by: Willem Melching * Cleanup Co-authored-by: Gijs Koning --- selfdrive/locationd/laikad.py | 103 +++++++++++++++++++++++++++++++--- 1 file changed, 95 insertions(+), 8 deletions(-) diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index ebe7404e17..8eb81345ff 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -7,16 +7,17 @@ from typing import List, Optional import numpy as np from collections import defaultdict +import sympy from numpy.linalg import linalg from cereal import log, messaging from common.params import Params, put_nonblocking from laika import AstroDog -from laika.constants import SECS_IN_HR, SECS_IN_MIN +from laika.constants import EARTH_ROTATION_RATE, SECS_IN_HR, SECS_IN_MIN, SPEED_OF_LIGHT from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_ephem from laika.gps_time import GPSTime from laika.helpers import ConstellationId -from laika.raw_gnss import GNSSMeasurement, calc_pos_fix, correct_measurements, process_measurements, read_raw_ublox +from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox from selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind from selfdrive.locationd.models.gnss_kf import GNSSKalman from selfdrive.locationd.models.gnss_kf import States as GStates @@ -38,6 +39,7 @@ class Laikad: self.last_cached_t = None self.save_ephemeris = save_ephemeris self.load_cache() + self.posfix_functions = {constellation: get_posfix_sympy_fun(constellation) for constellation in (ConstellationId.GPS, ConstellationId.GLONASS)} def load_cache(self): cache = Params().get(EPHEMERIS_CACHE) @@ -66,7 +68,9 @@ class Laikad: self.fetch_orbits(latest_msg_t + SECS_IN_MIN, block) new_meas = read_raw_ublox(report) processed_measurements = process_measurements(new_meas, self.astro_dog) - pos_fix = calc_pos_fix(processed_measurements, min_measurements=4) + + min_measurements = 5 if any(p.constellation_id == ConstellationId.GLONASS for p in processed_measurements) else 4 + pos_fix = calc_pos_fix_gauss_newton(processed_measurements, self.posfix_functions, min_measurements=min_measurements) t = ublox_mono_time * 1e-9 kf_pos_std = None @@ -84,7 +88,7 @@ class Laikad: if est_pos is not None: corrected_measurements = correct_measurements(processed_measurements, est_pos, self.astro_dog) - self.update_localizer(pos_fix, t, corrected_measurements) + self.update_localizer(est_pos, t, corrected_measurements) kf_valid = all(self.kf_valid(t)) ecef_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist() @@ -110,7 +114,7 @@ class Laikad: # elif ublox_msg.which == 'ionoData': # todo add this. Needed to better correct messages offline. First fix ublox_msg.cc to sent them. - def update_localizer(self, pos_fix, t: float, measurements: List[GNSSMeasurement]): + def update_localizer(self, est_pos, t: float, measurements: List[GNSSMeasurement]): # Check time and outputs are valid valid = self.kf_valid(t) if not all(valid): @@ -123,11 +127,10 @@ class Laikad: else: cloudlog.error("Gnss kalman std too far") - if len(pos_fix) == 0: + if est_pos is None: cloudlog.info("Position fix not available when resetting kalman filter") return - post_est = pos_fix[0][:3].tolist() - self.init_gnss_localizer(post_est) + self.init_gnss_localizer(est_pos.tolist()) if len(measurements) > 0: kf_add_observations(self.gnss_kf, t, measurements) else: @@ -227,6 +230,90 @@ def deserialize_hook(dct): return dct +def calc_pos_fix_gauss_newton(measurements, posfix_functions, x0=None, signal='C1C', min_measurements=6): + ''' + Calculates gps fix using gauss newton method + To solve the problem a minimal of 4 measurements are required. + If Glonass is included 5 are required to solve for the additional free variable. + returns: + 0 -> list with positions + ''' + if x0 is None: + x0 = [0, 0, 0, 0, 0] + n = len(measurements) + if n < min_measurements: + return [] + + Fx_pos = pr_residual(measurements, posfix_functions, signal=signal) + x = gauss_newton(Fx_pos, x0) + residual, _ = Fx_pos(x, weight=1.0) + return x, residual + + +def pr_residual(measurements, posfix_functions, signal='C1C'): + def Fx_pos(inp, weight=None): + vals, gradients = [], [] + + for meas in measurements: + pr = meas.observables[signal] + pr += meas.sat_clock_err * SPEED_OF_LIGHT + + w = (1 / meas.observables_std[signal]) if weight is None else weight + + val, *gradient = posfix_functions[meas.constellation_id](*inp, pr, *meas.sat_pos, w) + vals.append(val) + gradients.append(gradient) + return np.asarray(vals), np.asarray(gradients) + + return Fx_pos + + +def gauss_newton(fun, b, xtol=1e-8, max_n=25): + for _ in range(max_n): + # Compute function and jacobian on current estimate + r, J = fun(b) + + # Update estimate + delta = np.linalg.pinv(J) @ r + b -= delta + + # Check step size for stopping condition + if np.linalg.norm(delta) < xtol: + break + return b + + +def get_posfix_sympy_fun(constellation): + # Unknowns + x, y, z = sympy.Symbol('x'), sympy.Symbol('y'), sympy.Symbol('z') + bc = sympy.Symbol('bc') + bg = sympy.Symbol('bg') + var = [x, y, z, bc, bg] + + # Knowns + pr = sympy.Symbol('pr') + sat_x, sat_y, sat_z = sympy.Symbol('sat_x'), sympy.Symbol('sat_y'), sympy.Symbol('sat_z') + weight = sympy.Symbol('weight') + + theta = EARTH_ROTATION_RATE * (pr - bc) / SPEED_OF_LIGHT + val = sympy.sqrt( + (sat_x * sympy.cos(theta) + sat_y * sympy.sin(theta) - x) ** 2 + + (sat_y * sympy.cos(theta) - sat_x * sympy.sin(theta) - y) ** 2 + + (sat_z - z) ** 2 + ) + + if constellation == ConstellationId.GLONASS: + res = weight * (val - (pr - bc - bg)) + elif constellation == ConstellationId.GPS: + res = weight * (val - (pr - bc)) + else: + raise NotImplementedError(f"Constellation {constellation} not supported") + + res = [res] + [sympy.diff(res, v) for v in var] + + return sympy.lambdify([x, y, z, bc, bg, pr, sat_x, sat_y, sat_z, weight], res) + + def main(): sm = messaging.SubMaster(['ubloxGnss']) pm = messaging.PubMaster(['gnssMeasurements']) From efc8aa05b4573f9ac96d518fbf04b6d799e487d0 Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Wed, 15 Jun 2022 11:32:26 -0700 Subject: [PATCH 89/93] Update model ref --- selfdrive/test/process_replay/model_replay_ref_commit | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index a7a909d2a4..ec5ef11311 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -512a9d4596c8faba304d6f7ded2ce77837357b65 +6ec6db2f2a070ba1603bbe1d934509781cc4556a From c851cf737931cba16909b2fe20ffec4858acd374 Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Wed, 15 Jun 2022 11:33:59 -0700 Subject: [PATCH 90/93] Revert "Update model ref" This reverts commit efc8aa05b4573f9ac96d518fbf04b6d799e487d0. --- selfdrive/test/process_replay/model_replay_ref_commit | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index ec5ef11311..a7a909d2a4 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -6ec6db2f2a070ba1603bbe1d934509781cc4556a +512a9d4596c8faba304d6f7ded2ce77837357b65 From 9283040d847b120fdf7759d5bd12000863e12f73 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Wed, 15 Jun 2022 15:29:42 -0700 Subject: [PATCH 91/93] Rocket league model (#24869) * dd9a502d-c8e2-4831-b365-804b0ae0739d/600 80041070-d276-4fed-bdb9-0075e5442908/420 * no elementwise op * 9dabf0fe-2e60-44bf-8d3a-d20a74aca072/600 ae746590-0bb5-4a16-80db-15f02d314f03/300 c4663a12-b499-4c9b-90dd-b169e3948cb1/60 * explicit slice * some copies are useful * 1456d261-d232-4654-8885-4d9fde883894/440 c06eba55-1931-4e00-9d63-acad00161be0/700 af2eb6ba-1935-4318-aaf8-868db81a4932/425 * 154f663e-d3e9-4020-ad49-0e640588ebbe/399 badb5e69-504f-4544-a99e-ba75ed204b74/800 08330327-7663-4874-af7a-dcbd2c994ba7/800 * set steer rate cost to 1.0 * smaller temporal size * Update model reg * update model ref again * This did upload somehow * Update steer rate cost Co-authored-by: Yassine Yousfi --- selfdrive/car/body/interface.py | 1 - selfdrive/car/chrysler/interface.py | 1 - selfdrive/car/ford/interface.py | 1 - selfdrive/car/gm/interface.py | 1 - selfdrive/car/honda/interface.py | 1 - selfdrive/car/hyundai/interface.py | 1 - selfdrive/car/mazda/interface.py | 1 - selfdrive/car/nissan/interface.py | 1 - selfdrive/car/subaru/interface.py | 1 - selfdrive/car/tesla/interface.py | 1 - selfdrive/car/tests/test_car_interfaces.py | 1 - selfdrive/car/tests/test_models.py | 1 - selfdrive/car/toyota/interface.py | 1 - selfdrive/car/volkswagen/interface.py | 1 - selfdrive/controls/lib/lateral_planner.py | 9 ++++----- selfdrive/controls/plannerd.py | 2 +- selfdrive/modeld/models/driving.h | 2 +- selfdrive/modeld/models/supercombo.dlc | 4 ++-- selfdrive/modeld/models/supercombo.onnx | 4 ++-- selfdrive/modeld/thneed/compile.cc | 2 +- selfdrive/modeld/thneed/optimizer.cc | 8 ++++---- selfdrive/test/process_replay/model_replay_ref_commit | 2 +- selfdrive/test/process_replay/ref_commit | 2 +- 23 files changed, 17 insertions(+), 32 deletions(-) diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py index 3d6bd5e22a..e85735b1a6 100644 --- a/selfdrive/car/body/interface.py +++ b/selfdrive/car/body/interface.py @@ -19,7 +19,6 @@ class CarInterface(CarInterfaceBase): ret.minSteerSpeed = -math.inf ret.maxLateralAccel = math.inf # TODO: set to a reasonable value ret.steerRatio = 0.5 - ret.steerRateCost = 0.5 ret.steerLimitTimer = 1.0 ret.steerActuatorDelay = 0. diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 55672dd4ab..817a5b387e 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -20,7 +20,6 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15, 0.30], [0.03, 0.05]] ret.lateralTuning.pid.kf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 ret.steerActuatorDelay = 0.1 - ret.steerRateCost = 0.7 ret.steerLimitTimer = 0.4 if candidate in (CAR.JEEP_CHEROKEE, CAR.JEEP_CHEROKEE_2019): diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index ec7c723669..c608cc08d9 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -59,7 +59,6 @@ class CarInterface(CarInterfaceBase): # LCA can steer down to zero ret.minSteerSpeed = 0. - ret.steerRateCost = 1.0 ret.centerToFront = ret.wheelbase * 0.44 ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 498f4b3347..9c1744dfb4 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -63,7 +63,6 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]] ret.lateralTuning.pid.kf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594 - ret.steerRateCost = 1.0 ret.steerActuatorDelay = 0.1 # Default delay, not measured yet ret.longitudinalTuning.kpBP = [5., 35.] diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index ea7deab5d7..994152608e 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -319,7 +319,6 @@ class CarInterface(CarInterfaceBase): tire_stiffness_factor=tire_stiffness_factor) ret.steerActuatorDelay = 0.1 - ret.steerRateCost = 0.5 ret.steerLimitTimer = 0.8 return ret diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 6fd75ebfab..497d6b3f30 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -43,7 +43,6 @@ class CarInterface(CarInterfaceBase): ret.dashcamOnly = not os.path.exists('/data/enable-ev6') ret.steerActuatorDelay = 0.1 # Default delay - ret.steerRateCost = 0.5 ret.steerLimitTimer = 0.4 tire_stiffness_factor = 1. diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 0ef573c6b6..cbeb910de2 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -25,7 +25,6 @@ class CarInterface(CarInterfaceBase): ret.dashcamOnly = candidate not in (CAR.CX5_2022, CAR.CX9_2021) ret.steerActuatorDelay = 0.1 - ret.steerRateCost = 1.0 ret.steerLimitTimer = 0.8 tire_stiffness_factor = 0.70 # not optimized yet diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 3bf34e8266..436cef68bf 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -14,7 +14,6 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.nissan)] ret.steerLimitTimer = 1.0 - ret.steerRateCost = 0.5 ret.steerActuatorDelay = 0.1 diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index d0d8e91ce1..8c5cab86e8 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -22,7 +22,6 @@ class CarInterface(CarInterfaceBase): ret.dashcamOnly = candidate in PREGLOBAL_CARS - ret.steerRateCost = 0.7 ret.steerLimitTimer = 0.4 if candidate == CAR.ASCENT: diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 71594cecb6..d4eda38e64 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -42,7 +42,6 @@ class CarInterface(CarInterfaceBase): ret.steerLimitTimer = 1.0 ret.steerActuatorDelay = 0.25 - ret.steerRateCost = 0.5 if candidate in (CAR.AP2_MODELS, CAR.AP1_MODELS): ret.mass = 2100. + STD_CARGO_KG diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 52c89440c7..15df1aafef 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -33,7 +33,6 @@ class TestCarInterfaces(unittest.TestCase): assert car_interface self.assertGreater(car_params.mass, 1) - self.assertGreater(car_params.steerRateCost, 1e-3) if car_params.steerControlType != car.CarParams.SteerControlType.angle: tuning = car_params.lateralTuning.which() diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index 9ab881279b..27dc4ba776 100755 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -121,7 +121,6 @@ class TestCarModelBase(unittest.TestCase): # make sure car params are within a valid range self.assertGreater(self.CP.mass, 1) - self.assertGreater(self.CP.steerRateCost, 1e-3) if self.CP.steerControlType != car.CarParams.SteerControlType.angle: tuning = self.CP.lateralTuning.which() diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 7969775a89..d0f4789775 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -213,7 +213,6 @@ class CarInterface(CarInterfaceBase): ret.mass = 4305. * CV.LB_TO_KG + STD_CARGO_KG set_lat_tune(ret.lateralTuning, LatTunes.PID_J) - ret.steerRateCost = 1. ret.centerToFront = ret.wheelbase * 0.44 # TODO: get actual value, for now starting with reasonable value for diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 3a0d7c8ce8..6782105c16 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -45,7 +45,6 @@ class CarInterface(CarInterfaceBase): # Global lateral tuning defaults, can be overridden per-vehicle ret.steerActuatorDelay = 0.1 - ret.steerRateCost = 1.0 ret.steerLimitTimer = 0.4 ret.steerRatio = 15.6 # Let the params learner figure this out tire_stiffness_factor = 1.0 # Let the params learner figure this out diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index ec21c16e91..019a19fb1d 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -11,13 +11,12 @@ from cereal import log class LateralPlanner: - def __init__(self, CP, use_lanelines=True, wide_camera=False): + def __init__(self, use_lanelines=True, wide_camera=False): self.use_lanelines = use_lanelines self.LP = LanePlanner(wide_camera) self.DH = DesireHelper() self.last_cloudlog_t = 0 - self.steer_rate_cost = CP.steerRateCost self.solution_invalid_cnt = 0 self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3)) @@ -59,12 +58,12 @@ class LateralPlanner: # Calculate final driving path and set MPC costs if self.use_lanelines: d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz) - self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, self.steer_rate_cost) + self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, MPC_COST_LAT.STEER_RATE) else: d_path_xyz = self.path_xyz # Heading cost is useful at low speed, otherwise end of plan can be off-heading heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.15]) - self.lat_mpc.set_weights(MPC_COST_LAT.PATH, heading_cost, self.steer_rate_cost) + self.lat_mpc.set_weights(MPC_COST_LAT.PATH, heading_cost, MPC_COST_LAT.STEER_RATE) y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1]) heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw) @@ -79,7 +78,7 @@ class LateralPlanner: y_pts, heading_pts) # init state for next - # mpc.u_sol is the desired curvature rate given x0 curv state. + # mpc.u_sol is the desired curvature rate given x0 curv state. # with x0[3] = measured_curvature, this would be the actual desired rate. # instead, interpolate x_sol so that x0[3] is the desired curvature for lat_control. self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3]) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 06807b2a5c..9356a55d84 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -22,7 +22,7 @@ def plannerd_thread(sm=None, pm=None): cloudlog.event("e2e mode", on=use_lanelines) longitudinal_planner = Planner(CP) - lateral_planner = LateralPlanner(CP, use_lanelines=use_lanelines, wide_camera=wide_camera) + lateral_planner = LateralPlanner(use_lanelines=use_lanelines, wide_camera=wide_camera) if sm is None: sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'modelV2'], diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index 97e65fbc0e..a691051636 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -245,7 +245,7 @@ struct ModelOutput { constexpr int OUTPUT_SIZE = sizeof(ModelOutput) / sizeof(float); #ifdef TEMPORAL - constexpr int TEMPORAL_SIZE = 512; + constexpr int TEMPORAL_SIZE = 512+256; #else constexpr int TEMPORAL_SIZE = 0; #endif diff --git a/selfdrive/modeld/models/supercombo.dlc b/selfdrive/modeld/models/supercombo.dlc index 23f6d904fb..90f7a2e65b 100644 --- a/selfdrive/modeld/models/supercombo.dlc +++ b/selfdrive/modeld/models/supercombo.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:027cbb1fabae369878271cb0e3505071a8bdaa07473fad9a0b2e8d695c5dc1ff -size 76725611 +oid sha256:4c2cb3a3054f3292bbe538d6b793908dc2e234c200802d41b6766d3cb51b0b44 +size 101662751 diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index 9023c18dd7..0493398560 100644 --- a/selfdrive/modeld/models/supercombo.onnx +++ b/selfdrive/modeld/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:484976ea5bd4ddcabc82e95faf30d7311a27802c1e337472558699fa2395a499 -size 77472267 +oid sha256:96b60d0bfd1386c93b4f79195aa1c5e77b23e0250578a308ee2c58857ed5eb49 +size 102570834 diff --git a/selfdrive/modeld/thneed/compile.cc b/selfdrive/modeld/thneed/compile.cc index f76c63b2b9..a2f55ffd97 100644 --- a/selfdrive/modeld/thneed/compile.cc +++ b/selfdrive/modeld/thneed/compile.cc @@ -5,7 +5,7 @@ #include "selfdrive/modeld/thneed/thneed.h" #include "system/hardware/hw.h" -#define TEMPORAL_SIZE 512 +#define TEMPORAL_SIZE 512+256 #define DESIRE_LEN 8 #define TRAFFIC_CONVENTION_LEN 2 diff --git a/selfdrive/modeld/thneed/optimizer.cc b/selfdrive/modeld/thneed/optimizer.cc index 03d20ff386..39737d3d76 100644 --- a/selfdrive/modeld/thneed/optimizer.cc +++ b/selfdrive/modeld/thneed/optimizer.cc @@ -9,7 +9,7 @@ extern map g_program_source; -static int is_same_size_image(cl_mem a, cl_mem b) { +/*static int is_same_size_image(cl_mem a, cl_mem b) { size_t a_width, a_height, a_depth, a_array_size, a_row_pitch, a_slice_pitch; clGetImageInfo(a, CL_IMAGE_WIDTH, sizeof(a_width), &a_width, NULL); clGetImageInfo(a, CL_IMAGE_HEIGHT, sizeof(a_height), &a_height, NULL); @@ -29,7 +29,7 @@ static int is_same_size_image(cl_mem a, cl_mem b) { return (a_width == b_width) && (a_height == b_height) && (a_depth == b_depth) && (a_array_size == b_array_size) && (a_row_pitch == b_row_pitch) && (a_slice_pitch == b_slice_pitch); -} +}*/ static cl_mem make_image_like(cl_context context, cl_mem val) { cl_image_format format; @@ -138,7 +138,7 @@ int Thneed::optimize() { // delete useless copy layers // saves ~0.7 ms - if (kq[i]->name == "concatenation" || kq[i]->name == "flatten") { + /*if (kq[i]->name == "concatenation" || kq[i]->name == "flatten") { string in = kq[i]->args[kq[i]->get_arg_num("input")]; string out = kq[i]->args[kq[i]->get_arg_num("output")]; if (is_same_size_image(*(cl_mem*)in.data(), *(cl_mem*)out.data())) { @@ -148,7 +148,7 @@ int Thneed::optimize() { kq.erase(kq.begin()+i); --i; } - } + }*/ // NOTE: if activations/accumulation are done in the wrong order, this will be wrong diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index a7a909d2a4..54c2ed54ad 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -512a9d4596c8faba304d6f7ded2ce77837357b65 +629eaa7b26d1721a71547f9de880f99732cb27f3 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index b8976e5f38..8ed68d5bdd 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -1d66eed104dbc124c4e5679f5dddf40197b86ce9 +ed1dfb8b155ebcd8fdad4e06462b3bb7869fc67b From 725ccc01798c7f4b3278cc34525f99a6344e52aa Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Wed, 15 Jun 2022 18:01:02 -0700 Subject: [PATCH 92/93] HKG: simplify Kia K5 compatibility (#24810) LKAS/LFA is standard on the K5 --- docs/CARS.md | 2 +- selfdrive/car/hyundai/values.py | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/docs/CARS.md b/docs/CARS.md index 9b3322ac8e..c0bd62eb3f 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -95,7 +95,7 @@ How We Rate The Cars |Kia|Ceed 2019|SCC + LKAS|||||| |Kia|Forte 2018|SCC + LKAS|||||| |Kia|Forte 2019-21|SCC + LKAS|||||| -|Kia|K5 2021-22|SCC + LFA|||||| +|Kia|K5 2021-22|SCC|||||| |Kia|Niro Hybrid 2021|SCC + LKAS|||||| |Kia|Niro Hybrid 2022|SCC + LKAS|||||| |Kia|Optima 2019|SCC + LKAS|||||| diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 4ca81fdb40..df7ce1e6f4 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -80,6 +80,8 @@ class CAR: @dataclass class HyundaiCarInfo(CarInfo): + # TODO: we can probably remove LKAS. LKAS is standard on many + # HKG and for others, it's likely packaged together with SCC package: str = "SCC + LKAS" good_torque: bool = True @@ -121,7 +123,7 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = { HyundaiCarInfo("Kia Forte 2018", harness=Harness.hyundai_b), HyundaiCarInfo("Kia Forte 2019-21", harness=Harness.hyundai_g), ], - CAR.KIA_K5_2021: HyundaiCarInfo("Kia K5 2021-22", "SCC + LFA", harness=Harness.hyundai_a), + CAR.KIA_K5_2021: HyundaiCarInfo("Kia K5 2021-22", "SCC", harness=Harness.hyundai_a), CAR.KIA_NIRO_EV: [ HyundaiCarInfo("Kia Niro Electric 2019-20", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_f), HyundaiCarInfo("Kia Niro Electric 2021", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_c), From e910ce87a44884a3f216fbced306e8d3124a04de Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 15 Jun 2022 21:42:54 -0700 Subject: [PATCH 93/93] thermald: fix panda dropout when we miss a pandaStates (#24870) immediate fix for "panda dropout" --- 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 b909d1198c..6cf5c428a8 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -220,7 +220,7 @@ def thermald_thread(end_event, hw_queue): if TICI: fan_controller = TiciFanController() - elif (sec_since_boot() - sm.rcv_time['pandaStates']/1e9) > DISCONNECT_TIMEOUT: + elif (sec_since_boot() - sm.rcv_time['pandaStates']) > DISCONNECT_TIMEOUT: if onroad_conditions["ignition"]: onroad_conditions["ignition"] = False cloudlog.error("panda timed out onroad")