From 7a815418d600bed1e2500b4fdbe4f1958f432555 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 20 Jun 2022 12:12:18 +0200 Subject: [PATCH 1/4] bump cereal --- cereal | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cereal b/cereal index 6faf34064b..f445245a8f 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 6faf34064b70ab98c241d4a1cd9006b09ecaadfc +Subproject commit f445245a8fbe01c33a372d82897575e7acc08ffc From e2757208622e29a00d5191d7d2f971cafcf61602 Mon Sep 17 00:00:00 2001 From: Robbe Derks Date: Mon, 20 Jun 2022 12:15:39 +0200 Subject: [PATCH 2/4] Add new message to Tesla AP2 fingerprint (#24912) * add new message * fix spacing * add space --- selfdrive/car/tesla/values.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py index 2ed4c963ee..296169587a 100644 --- a/selfdrive/car/tesla/values.py +++ b/selfdrive/car/tesla/values.py @@ -22,7 +22,7 @@ CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = { FINGERPRINTS = { CAR.AP2_MODELS: [ { - 1: 8, 3: 8, 14: 8, 21: 4, 69: 8, 109: 4, 257: 3, 264: 8, 277: 6, 280: 6, 293: 4, 296: 4, 309: 5, 325: 8, 328: 5, 336: 8, 341: 8, 360: 7, 373: 8, 389: 8, 415: 8, 513: 5, 516: 8, 518: 8, 520: 4, 522: 8, 524: 8, 526: 8, 532: 3, 536: 8, 537: 3, 538: 8, 542: 8, 551: 5, 552: 2, 556: 8, 558: 8, 568: 8, 569: 8, 574: 8, 576: 3, 577: 8, 582: 5, 583: 8, 584: 4, 585: 8, 590: 8, 601: 8, 606: 8, 608: 1, 622: 8, 627: 6, 638: 8, 641: 8, 643: 8, 692: 8, 693: 8, 695: 8, 696: 8, 697: 8, 699: 8, 700: 8, 701: 8, 702: 8, 703: 8, 704: 8, 708: 8, 709: 8, 710: 8, 711: 8, 712: 8, 728: 8, 744: 8, 760: 8, 772: 8, 775: 8, 776: 8, 777: 8, 778: 8, 782: 8, 788: 8, 791: 8, 792: 8, 796: 2, 797: 8, 798: 6, 799: 8, 804: 8, 805: 8, 807: 8, 808: 1, 811: 8, 812: 8, 813: 8, 814: 5, 815: 8, 820: 8, 823: 8, 824: 8, 829: 8, 830: 5, 836: 8, 840: 8, 845: 8, 846: 5, 848: 8, 852: 8, 853: 8, 856: 4, 857: 6, 861: 8, 862: 5, 872: 8, 876: 8, 877: 8, 879: 8, 880: 8, 882: 8, 884: 8, 888: 8, 893: 8, 894: 8, 901: 6, 904: 3, 905: 8, 906: 8, 908: 2, 909: 8, 910: 8, 912: 8, 920: 8, 921: 8, 925: 4, 926: 6, 936: 8, 941: 8, 949: 8, 952: 8, 953: 6, 968: 8, 969: 6, 970: 8, 971: 8, 977: 8, 984: 8, 987: 8, 990: 8, 1000: 8, 1001: 8, 1006: 8, 1007: 8, 1008: 8, 1010: 6, 1014: 1, 1015: 8, 1016: 8, 1017: 8, 1018: 8, 1020: 8, 1026: 8, 1028: 8, 1029: 8, 1030: 8, 1032: 1, 1033: 1, 1034: 8, 1048: 1, 1049: 8, 1061: 8, 1064: 8, 1065: 8, 1070: 8, 1080: 8, 1081: 8, 1097: 8, 1113: 8, 1129: 8, 1145: 8, 1160: 4, 1177: 8, 1281: 8, 1328: 8, 1329: 8, 1332: 8, 1335: 8, 1337: 8, 1353: 8, 1368: 8, 1412: 8, 1436: 8, 1476: 8, 1481: 8, 1497: 8, 1513: 8, 1519: 8, 1601: 8, 1605: 8, 1617: 8, 1621: 8, 1625: 8, 1800: 4, 1804: 8, 1812: 8, 1815: 8, 1816: 8, 1824: 8, 1828: 8, 1831: 8, 1832: 8, 1840: 8, 1848: 8, 1864: 8, 1880: 8, 1892: 8, 1896: 8, 1912: 8, 1960: 8, 1992: 8, 2008: 3, 2015: 8, 2043: 5, 2045: 4 + 1: 8, 3: 8, 14: 8, 21: 4, 69: 8, 109: 4, 257: 3, 264: 8, 277: 6, 280: 6, 293: 4, 296: 4, 309: 5, 325: 8, 328: 5, 336: 8, 341: 8, 360: 7, 373: 8, 389: 8, 415: 8, 513: 5, 516: 8, 518: 8, 520: 4, 522: 8, 524: 8, 526: 8, 532: 3, 536: 8, 537: 3, 538: 8, 542: 8, 551: 5, 552: 2, 556: 8, 558: 8, 568: 8, 569: 8, 574: 8, 576: 3, 577: 8, 582: 5, 583: 8, 584: 4, 585: 8, 590: 8, 601: 8, 606: 8, 608: 1, 622: 8, 627: 6, 638: 8, 641: 8, 643: 8, 692: 8, 693: 8, 695: 8, 696: 8, 697: 8, 699: 8, 700: 8, 701: 8, 702: 8, 703: 8, 704: 8, 708: 8, 709: 8, 710: 8, 711: 8, 712: 8, 728: 8, 744: 8, 760: 8, 772: 8, 775: 8, 776: 8, 777: 8, 778: 8, 782: 8, 788: 8, 791: 8, 792: 8, 796: 2, 797: 8, 798: 6, 799: 8, 804: 8, 805: 8, 807: 8, 808: 1, 811: 8, 812: 8, 813: 8, 814: 5, 815: 8, 820: 8, 823: 8, 824: 8, 829: 8, 830: 5, 836: 8, 840: 8, 845: 8, 846: 5, 848: 8, 852: 8, 853: 8, 856: 4, 857: 6, 861: 8, 862: 5, 872: 8, 876: 8, 877: 8, 879: 8, 880: 8, 882: 8, 884: 8, 888: 8, 893: 8, 894: 8, 901: 6, 904: 3, 905: 8, 906: 8, 908: 2, 909: 8, 910: 8, 912: 8, 920: 8, 921: 8, 925: 4, 926: 6, 936: 8, 941: 8, 949: 8, 952: 8, 953: 6, 968: 8, 969: 6, 970: 8, 971: 8, 977: 8, 984: 8, 987: 8, 990: 8, 1000: 8, 1001: 8, 1006: 8, 1007: 8, 1008: 8, 1010: 6, 1014: 1, 1015: 8, 1016: 8, 1017: 8, 1018: 8, 1020: 8, 1026: 8, 1028: 8, 1029: 8, 1030: 8, 1032: 1, 1033: 1, 1034: 8, 1048: 1, 1049: 8, 1061: 8, 1064: 8, 1065: 8, 1070: 8, 1080: 8, 1081: 8, 1097: 8, 1113: 8, 1129: 8, 1145: 8, 1160: 4, 1177: 8, 1281: 8, 1328: 8, 1329: 8, 1332: 8, 1335: 8, 1337: 8, 1353: 8, 1368: 8, 1412: 8, 1436: 8, 1476: 8, 1481: 8, 1497: 8, 1513: 8, 1519: 8, 1601: 8, 1605: 8, 1617: 8, 1621: 8, 1625: 8, 1665: 8, 1800: 4, 1804: 8, 1812: 8, 1815: 8, 1816: 8, 1824: 8, 1828: 8, 1831: 8, 1832: 8, 1840: 8, 1848: 8, 1864: 8, 1880: 8, 1892: 8, 1896: 8, 1912: 8, 1960: 8, 1992: 8, 2008: 3, 2015: 8, 2043: 5, 2045: 4 }, ], CAR.AP1_MODELS: [ From 1c4b145aa4d85b7107254be6618592781ff7323e Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Mon, 20 Jun 2022 14:24:11 +0200 Subject: [PATCH 3/4] Improve laikad cpu usage. Less pos fix. Less local imports (#24887) * Improve laikad cpu usage. Less pos fix. Not local imports * Add laika to files_common * Add laika to dockerfile sim * Fix * Fix * undo --- release/files_common | 1 + selfdrive/locationd/laikad.py | 35 ++++++++++++---------- selfdrive/locationd/models/gnss_helpers.py | 4 +-- tools/sim/Dockerfile.sim | 1 + 4 files changed, 23 insertions(+), 18 deletions(-) diff --git a/release/files_common b/release/files_common index ecb71b4e69..7274ee9bf6 100644 --- a/release/files_common +++ b/release/files_common @@ -414,6 +414,7 @@ pyextra/.gitignore pyextra/acados_template/** rednose/** +laika/** cereal/.gitignore cereal/__init__.py diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 6fd70875ac..f1997bceb0 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -39,7 +39,9 @@ class Laikad: self.save_ephemeris = save_ephemeris self.load_cache() self.posfix_functions = {constellation: get_posfix_sympy_fun(constellation) for constellation in (ConstellationId.GPS, ConstellationId.GLONASS)} - self.last_pos_fix = last_known_position + self.last_pos_fix = last_known_position if last_known_position is not None else [] + self.last_pos_residual = [] + self.last_pos_fix_t = None def load_cache(self): cache = Params().get(EPHEMERIS_CACHE) @@ -62,23 +64,26 @@ class Laikad: def process_ublox_msg(self, ublox_msg, ublox_mono_time: int, block=False): if ublox_msg.which == 'measurementReport': + t = ublox_mono_time * 1e-9 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) + new_meas = read_raw_ublox(report) processed_measurements = process_measurements(new_meas, self.astro_dog) - min_measurements = 5 if any(p.constellation_id == ConstellationId.GLONASS for p in processed_measurements) else 4 - pos_fix, pos_fix_residual = calc_pos_fix_gauss_newton(processed_measurements, self.posfix_functions, min_measurements=min_measurements) - if len(pos_fix) > 0: - self.last_pos_fix = pos_fix[:3] - est_pos = self.last_pos_fix + if self.last_pos_fix_t is None or abs(self.last_pos_fix_t - t) >= 2: + min_measurements = 5 if any(p.constellation_id == ConstellationId.GLONASS for p in processed_measurements) else 4 + pos_fix, pos_fix_residual = calc_pos_fix_gauss_newton(processed_measurements, self.posfix_functions, min_measurements=min_measurements) + if len(pos_fix) > 0: + self.last_pos_fix = pos_fix[:3] + self.last_pos_residual = pos_fix_residual + self.last_pos_fix_t = t - corrected_measurements = correct_measurements(processed_measurements, est_pos, self.astro_dog) if est_pos is not None else [] + corrected_measurements = correct_measurements(processed_measurements, self.last_pos_fix, self.astro_dog) if self.last_pos_fix_t is not None else [] - t = ublox_mono_time * 1e-9 - self.update_localizer(est_pos, t, corrected_measurements) + self.update_localizer(self.last_pos_fix, t, corrected_measurements) 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() @@ -94,7 +99,7 @@ class Laikad: "gpsTimeOfWeek": report.rcvTow, "positionECEF": measurement_msg(value=ecef_pos, std=pos_std, valid=kf_valid), "velocityECEF": measurement_msg(value=ecef_vel, std=vel_std, valid=kf_valid), - "positionFixECEF": measurement_msg(value=pos_fix, std=pos_fix_residual, valid=len(pos_fix) > 0), + "positionFixECEF": measurement_msg(value=self.last_pos_fix, std=self.last_pos_residual, valid=self.last_pos_fix_t == t), "ubloxMonoTime": ublox_mono_time, "correctedMeasurements": meas_msgs } @@ -116,7 +121,7 @@ class Laikad: cloudlog.error("Time gap of over 10s detected, gnss kalman reset") elif not valid[2]: cloudlog.error("Gnss kalman filter state is nan") - if est_pos is not None: + if len(est_pos) > 0: cloudlog.info(f"Reset kalman filter with {est_pos}") self.init_gnss_localizer(est_pos) else: @@ -211,13 +216,13 @@ def kf_add_observations(gnss_kf: GNSSKalman, t: float, measurements: List[GNSSMe m_arr = m.as_array() if m.constellation_id == ConstellationId.GPS: ekf_data[ObservationKind.PSEUDORANGE_GPS].append(m_arr) - ekf_data[ObservationKind.PSEUDORANGE_RATE_GPS].append(m_arr) elif m.constellation_id == ConstellationId.GLONASS: ekf_data[ObservationKind.PSEUDORANGE_GLONASS].append(m_arr) - ekf_data[ObservationKind.PSEUDORANGE_RATE_GLONASS].append(m_arr) - + ekf_data[ObservationKind.PSEUDORANGE_RATE_GPS] = ekf_data[ObservationKind.PSEUDORANGE_GPS] + ekf_data[ObservationKind.PSEUDORANGE_RATE_GLONASS] = ekf_data[ObservationKind.PSEUDORANGE_GLONASS] for kind, data in ekf_data.items(): - gnss_kf.predict_and_observe(t, kind, data) + if len(data) >0: + gnss_kf.predict_and_observe(t, kind, data) class CacheSerializer(json.JSONEncoder): diff --git a/selfdrive/locationd/models/gnss_helpers.py b/selfdrive/locationd/models/gnss_helpers.py index a3bcbc0002..b6c1771ec6 100644 --- a/selfdrive/locationd/models/gnss_helpers.py +++ b/selfdrive/locationd/models/gnss_helpers.py @@ -1,16 +1,14 @@ import numpy as np +from laika.raw_gnss import GNSSMeasurement def parse_prr(m): - from laika.raw_gnss import GNSSMeasurement sat_pos_vel_i = np.concatenate((m[GNSSMeasurement.SAT_POS], m[GNSSMeasurement.SAT_VEL])) R_i = np.atleast_2d(m[GNSSMeasurement.PRR_STD]**2) z_i = m[GNSSMeasurement.PRR] return z_i, R_i, sat_pos_vel_i - def parse_pr(m): - from laika.raw_gnss import GNSSMeasurement pseudorange = m[GNSSMeasurement.PR] pseudorange_stdev = m[GNSSMeasurement.PR_STD] sat_pos_freq_i = np.concatenate((m[GNSSMeasurement.SAT_POS], diff --git a/tools/sim/Dockerfile.sim b/tools/sim/Dockerfile.sim index 0d6e8e584c..3692d48e0f 100644 --- a/tools/sim/Dockerfile.sim +++ b/tools/sim/Dockerfile.sim @@ -18,6 +18,7 @@ COPY ./third_party $HOME/openpilot/third_party COPY ./pyextra $HOME/openpilot/pyextra COPY ./site_scons $HOME/openpilot/site_scons COPY ./rednose $HOME/openpilot/rednose +COPY ./laika $HOME/openpilot/laika COPY ./common $HOME/openpilot/common COPY ./opendbc $HOME/openpilot/opendbc COPY ./cereal $HOME/openpilot/cereal From 8270c1c1a3584c1244f12143fc1314cdbbb05541 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 20 Jun 2022 19:14:43 +0200 Subject: [PATCH 4/4] ui: onroad widget check if sockets alive (#24913) * ui: onroad widget check if socket alive * cleanup --- selfdrive/ui/qt/onroad.cc | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 84b538411b..45bf41ba2d 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -172,22 +172,28 @@ NvgWindow::NvgWindow(VisionStreamType type, QWidget* parent) : fps_filter(UI_FRE void NvgWindow::updateState(const UIState &s) { const int SET_SPEED_NA = 255; const SubMaster &sm = *(s.sm); + + const bool cs_alive = sm.alive("controlsState"); + const bool nav_alive = sm.alive("navInstruction") && sm["navInstruction"].getValid(); + const auto cs = sm["controlsState"].getControlsState(); - float set_speed = cs.getVCruise(); + float set_speed = cs_alive ? cs.getVCruise() : SET_SPEED_NA; bool cruise_set = set_speed > 0 && (int)set_speed != SET_SPEED_NA; if (cruise_set && !s.scene.is_metric) { set_speed *= KM_TO_MILE; } - float cur_speed = std::max(0.0, sm["carState"].getCarState().getVEgo() * (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH)); + + float cur_speed = cs_alive ? std::max(0.0, sm["carState"].getCarState().getVEgo()) : 0.0; + cur_speed *= 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; + float speed_limit = nav_alive ? 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", sm["navInstruction"].getValid() && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD); - setProperty("has_eu_speed_limit", sm["navInstruction"].getValid() && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA); + setProperty("has_us_speed_limit", nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD); + setProperty("has_eu_speed_limit", nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA); setProperty("is_cruise_set", cruise_set); setProperty("speed", cur_speed);