From 1ecf6f351c35de24affbb1e2cb5675aea1a36f10 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Fri, 7 Oct 2022 00:16:18 -0700 Subject: [PATCH 01/11] Divide by 0 bug fix lateral planner (#25995) * Divide by speed correctly * Update * Update lateral_planner.py * Update ref_commit --- selfdrive/controls/lib/lateral_planner.py | 15 +++++++++------ selfdrive/test/process_replay/ref_commit | 2 +- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 48c98e7350..10e3edbebe 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -75,10 +75,10 @@ class LateralPlanner: y_pts, heading_pts, yaw_rate_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. + # init state for next iteration + # mpc.u_sol is the desired second derivative of psi given x0 curv state. + # with x0[3] = measured_yaw_rate, this would be the actual desired yaw rate. + # instead, interpolate x_sol so that x0[3] is the desired yaw rate 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 @@ -105,8 +105,11 @@ class LateralPlanner: lateralPlan.modelMonoTime = sm.logMonoTime['modelV2'] lateralPlan.dPathPoints = self.y_pts.tolist() lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist() - lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3]/sm['carState'].vEgo).tolist() - lateralPlan.curvatureRates = [float(x) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0] + + # clip speed for curv calculation at 1m/s, to prevent low speed extremes + clipped_speed = max(1.0, sm['carState'].vEgo) + lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3]/clipped_speed).tolist() + lateralPlan.curvatureRates = [float(x/clipped_speed) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0] lateralPlan.mpcSolutionValid = bool(plan_solution_valid) lateralPlan.solverExecutionTime = self.lat_mpc.solve_time diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 0e3cdf797f..78367235e6 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -e0ffcae8def2fd9c82c547d1f257d4f06a48a3c3 +f9536e41a6a160bdaa29d42bb164b0e4033857e5 From cfabae28e7554c12c0fcb4beab8b7b3353140316 Mon Sep 17 00:00:00 2001 From: pbkompasz <47194071+pbkompasz@users.noreply.github.com> Date: Fri, 7 Oct 2022 18:41:14 +0300 Subject: [PATCH 02/11] README -> README.md (#26005) --- tools/cabana/{README => README.md} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename tools/cabana/{README => README.md} (100%) diff --git a/tools/cabana/README b/tools/cabana/README.md similarity index 100% rename from tools/cabana/README rename to tools/cabana/README.md From 1f5187892f0854bbc6a7d7799fa0d9ca2284714a Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 7 Oct 2022 10:39:11 -0700 Subject: [PATCH 03/11] safer modem manager commands (#25999) Co-authored-by: Comma Device --- selfdrive/sensord/rawgps/rawgpsd.py | 4 ++-- selfdrive/sensord/rawgps/test_rawgps.py | 2 +- system/hardware/tici/hardware.py | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/sensord/rawgps/rawgpsd.py b/selfdrive/sensord/rawgps/rawgpsd.py index 5149ab6473..c430acc5e5 100755 --- a/selfdrive/sensord/rawgps/rawgpsd.py +++ b/selfdrive/sensord/rawgps/rawgpsd.py @@ -88,7 +88,7 @@ def try_setup_logs(diag, log_types): def mmcli(cmd: str) -> None: for _ in range(5): try: - subprocess.check_call(f"mmcli -m 0 {cmd}", shell=True) + subprocess.check_call(f"mmcli -m any --timeout 30 {cmd}", shell=True) break except subprocess.CalledProcessError: cloudlog.exception("rawgps.mmcli_command_failed") @@ -151,7 +151,7 @@ def main() -> NoReturn: # wait for ModemManager to come up cloudlog.warning("waiting for modem to come up") while True: - ret = subprocess.call("mmcli -m 0 --location-status", stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, shell=True) + ret = subprocess.call("mmcli -m any --timeout 10 --location-status", stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, shell=True) if ret == 0: break time.sleep(0.1) diff --git a/selfdrive/sensord/rawgps/test_rawgps.py b/selfdrive/sensord/rawgps/test_rawgps.py index f8dd3ad1e5..5bd0833955 100755 --- a/selfdrive/sensord/rawgps/test_rawgps.py +++ b/selfdrive/sensord/rawgps/test_rawgps.py @@ -40,7 +40,7 @@ class TestRawgpsd(unittest.TestCase): time.sleep(s) managed_processes['rawgpsd'].stop() - ls = subprocess.check_output("mmcli -m 0 --location-status --output-json", shell=True, encoding='utf-8') + ls = subprocess.check_output("mmcli -m any --location-status --output-json", shell=True, encoding='utf-8') loc_status = json.loads(ls) assert set(loc_status['modem']['location']['enabled']) <= {'3gpp-lac-ci'} diff --git a/system/hardware/tici/hardware.py b/system/hardware/tici/hardware.py index 340093b604..e2fd20c1be 100644 --- a/system/hardware/tici/hardware.py +++ b/system/hardware/tici/hardware.py @@ -480,7 +480,7 @@ class Tici(HardwareBase): # blue prime config if sim_id.startswith('8901410'): - os.system('mmcli -m 0 --3gpp-set-initial-eps-bearer-settings="apn=Broadband"') + os.system('mmcli -m any --3gpp-set-initial-eps-bearer-settings="apn=Broadband"') def get_networks(self): r = {} From 8dbb25e683039e2cd393b574e0fdaa0c894b1104 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Sat, 8 Oct 2022 02:28:09 +0800 Subject: [PATCH 04/11] params: remove all files if call clearAll with the ParamKeyType.ALL key. (#26000) * remove all files in dir * fix test case name --- common/params.cc | 11 +++++++---- common/tests/test_util.cc | 17 +++++++++++++++++ common/util.cc | 16 ++++++++++++++++ common/util.h | 1 + 4 files changed, 41 insertions(+), 4 deletions(-) diff --git a/common/params.cc b/common/params.cc index a23c0e003e..155bc88487 100644 --- a/common/params.cc +++ b/common/params.cc @@ -299,10 +299,13 @@ std::map Params::readAll() { void Params::clearAll(ParamKeyType key_type) { FileLock file_lock(params_path + "/.lock"); - std::string path; - for (auto &[key, type] : keys) { - if (type & key_type) { - unlink(getParamPath(key).c_str()); + if (key_type == ALL) { + util::remove_files_in_dir(getParamPath()); + } else { + for (auto &[key, type] : keys) { + if (type & key_type) { + unlink(getParamPath(key).c_str()); + } } } diff --git a/common/tests/test_util.cc b/common/tests/test_util.cc index 25ecf09aa9..b70cc9044a 100644 --- a/common/tests/test_util.cc +++ b/common/tests/test_util.cc @@ -143,3 +143,20 @@ TEST_CASE("util::create_directories") { REQUIRE(util::create_directories("", 0755) == false); } } + +TEST_CASE("util::remove_files_in_dir") { + std::string tmp_dir = "/tmp/test_remove_all_in_dir"; + system("rm /tmp/test_remove_all_in_dir -rf"); + REQUIRE(util::create_directories(tmp_dir, 0755)); + const int tmp_file_cnt = 10; + for (int i = 0; i < tmp_file_cnt; ++i) { + std::string tmp_file = tmp_dir + "/test_XXXXXX"; + int fd = mkstemp((char*)tmp_file.c_str()); + close(fd); + REQUIRE(util::file_exists(tmp_file.c_str())); + } + + REQUIRE(util::read_files_in_dir(tmp_dir).size() == tmp_file_cnt); + util::remove_files_in_dir(tmp_dir); + REQUIRE(util::read_files_in_dir(tmp_dir).empty()); +} diff --git a/common/util.cc b/common/util.cc index 92add63997..b6a8322a27 100644 --- a/common/util.cc +++ b/common/util.cc @@ -97,6 +97,22 @@ std::map read_files_in_dir(const std::string &path) { return ret; } +void remove_files_in_dir(const std::string &path) { + DIR *d = opendir(path.c_str()); + if (!d) return; + + std::string fn; + struct dirent *de = NULL; + while ((de = readdir(d))) { + if (de->d_type != DT_DIR) { + fn = path + "/" + de->d_name; + unlink(fn.c_str()); + } + } + + closedir(d); +} + int write_file(const char* path, const void* data, size_t size, int flags, mode_t mode) { int fd = HANDLE_EINTR(open(path, flags, mode)); if (fd == -1) { diff --git a/common/util.h b/common/util.h index f3a24723b4..e13f4dc130 100644 --- a/common/util.h +++ b/common/util.h @@ -80,6 +80,7 @@ std::string dir_name(std::string const& path); // **** file fhelpers ***** std::string read_file(const std::string& fn); std::map read_files_in_dir(const std::string& path); +void remove_files_in_dir(const std::string& path); int write_file(const char* path, const void* data, size_t size, int flags = O_WRONLY, mode_t mode = 0664); FILE* safe_fopen(const char* filename, const char* mode); From beb0f8ac234d28b72c6267d0b4c55fdf81c817b9 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Sat, 8 Oct 2022 02:28:24 +0800 Subject: [PATCH 05/11] cabana: custom slider to indicate the status of engaged/disengaged/alerts (#26003) * timeline * remove todo * cleanup --- tools/cabana/videowidget.cc | 46 ++++++++++++++++++++++++++++++++++++- tools/cabana/videowidget.h | 5 ++++ 2 files changed, 50 insertions(+), 1 deletion(-) diff --git a/tools/cabana/videowidget.cc b/tools/cabana/videowidget.cc index 26fb845340..7edbe5e38f 100644 --- a/tools/cabana/videowidget.cc +++ b/tools/cabana/videowidget.cc @@ -4,7 +4,9 @@ #include #include #include +#include #include +#include #include #include "tools/cabana/parser.h" @@ -98,8 +100,50 @@ void VideoWidget::updateState() { } // Slider -// TODO: show timeline bar like what replay did. Slider::Slider(QWidget *parent) : QSlider(Qt::Horizontal, parent) { + QTimer *timer = new QTimer(this); + timer->setInterval(2000); + timer->callOnTimeout([this]() { timeline = parser->replay->getTimeline(); }); + timer->start(); +} + +void Slider::paintEvent(QPaintEvent *ev) { + auto getPaintRange = [this](double begin, double end) -> std::pair { + double total_sec = maximum() - minimum(); + int start_pos = ((std::max((double)minimum(), (double)begin) - minimum()) / total_sec) * width(); + int end_pos = ((std::min((double)maximum(), (double)end) - minimum()) / total_sec) * width(); + return {start_pos, end_pos}; + }; + + QPainter p(this); + const int v_margin = 2; + p.fillRect(rect().adjusted(0, v_margin, 0, -v_margin), QColor(0, 0, 128)); + + for (auto [begin, end, type] : timeline) { + if (begin > maximum() || end < minimum()) continue; + + if (type == TimelineType::Engaged) { + auto [start_pos, end_pos] = getPaintRange(begin, end); + p.fillRect(QRect(start_pos, v_margin, end_pos - start_pos, height() - v_margin * 2), QColor(0, 135, 0)); + } + } + for (auto [begin, end, type] : timeline) { + if (type == TimelineType::Engaged || begin > maximum() || end < minimum()) continue; + + auto [start_pos, end_pos] = getPaintRange(begin, end); + if (type == TimelineType::UserFlag) { + p.fillRect(QRect(start_pos, height() - v_margin - 3, end_pos - start_pos, 3), Qt::white); + } else { + QColor color(Qt::green); + if (type != TimelineType::AlertInfo) + color = type == TimelineType::AlertWarning ? Qt::yellow : Qt::red; + + p.fillRect(QRect(start_pos, height() - v_margin - 3, end_pos - start_pos, 3), color); + } + } + p.setPen(QPen(Qt::black, 2)); + qreal x = width() * ((value() - minimum()) / double(maximum() - minimum())); + p.drawLine(QPointF{x, 0.}, QPointF{x, (qreal)height()}); } void Slider::mousePressEvent(QMouseEvent *e) { diff --git a/tools/cabana/videowidget.h b/tools/cabana/videowidget.h index a3605459e0..188456ecbe 100644 --- a/tools/cabana/videowidget.h +++ b/tools/cabana/videowidget.h @@ -5,6 +5,7 @@ #include #include "selfdrive/ui/qt/widgets/cameraview.h" +#include "tools/replay/replay.h" class Slider : public QSlider { Q_OBJECT @@ -15,6 +16,10 @@ public: signals: void setPosition(int value); + +private: + void paintEvent(QPaintEvent *ev) override; + std::vector> timeline; }; class VideoWidget : public QWidget { From 46e741c7cc652a9b1b266c0881ef6f9c30f5a226 Mon Sep 17 00:00:00 2001 From: Jason Young <46612682+jyoung8607@users.noreply.github.com> Date: Fri, 7 Oct 2022 15:50:35 -0400 Subject: [PATCH 06/11] VW MNB: Add EPS config script support (#26007) --- selfdrive/debug/vw_mqb_config.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/debug/vw_mqb_config.py b/selfdrive/debug/vw_mqb_config.py index c1068bf067..8c4dbc55ee 100755 --- a/selfdrive/debug/vw_mqb_config.py +++ b/selfdrive/debug/vw_mqb_config.py @@ -70,8 +70,8 @@ if __name__ == "__main__": coding_variant, current_coding_array, coding_byte, coding_bit = None, None, 0, 0 coding_length = len(current_coding) - # EV_SteerAssisMQB covers the majority of MQB racks (EPS_MQB_ZFLS) - if odx_file == "EV_SteerAssisMQB\x00": + # EV_SteerAssisMQB/MNB cover the majority of MQB racks (EPS_MQB_ZFLS) + if odx_file in ("EV_SteerAssisMQB\x00", "EV_SteerAssisMNB\x00"): coding_variant = "ZF" coding_byte = 0 coding_bit = 4 @@ -111,7 +111,7 @@ if __name__ == "__main__": if args.action in ["enable", "disable"]: print("\nAttempting configuration update") - assert(coding_variant in ("ZF", "APA")) + assert(coding_variant in ("ZF", "APA")) # ZF EPS config coding length can be anywhere from 1 to 4 bytes, but the # bit we care about is always in the same place in the first byte if args.action == "enable": From 8cee561dbfcb6e45f7ecd51adb0df67f4ebba908 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 7 Oct 2022 13:51:55 -0700 Subject: [PATCH 07/11] tombstoned: remove android code --- selfdrive/tombstoned.py | 43 +---------------------------------------- 1 file changed, 1 insertion(+), 42 deletions(-) diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index 0045e0766c..61a575f141 100755 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -62,47 +62,6 @@ def get_tombstones(): return files -def report_tombstone_android(fn): - f_size = os.path.getsize(fn) - if f_size > MAX_SIZE: - cloudlog.error(f"Tombstone {fn} too big, {f_size}. Skipping...") - return - - with open(fn, encoding='ISO-8859-1') as f: - contents = f.read() - - message = " ".join(contents.split('\n')[5:7]) - - # Cut off pid/tid, since that varies per run - name_idx = message.find('name') - if name_idx >= 0: - message = message[name_idx:] - - executable = "" - start_exe_idx = message.find('>>> ') - end_exe_idx = message.find(' <<<') - if start_exe_idx >= 0 and end_exe_idx >= 0: - executable = message[start_exe_idx + 4:end_exe_idx] - - # Cut off fault addr - fault_idx = message.find(', fault addr') - if fault_idx >= 0: - message = message[:fault_idx] - - sentry.report_tombstone(fn, message, contents) - - # Copy crashlog to upload folder - clean_path = executable.replace('./', '').replace('/', '_') - date = datetime.datetime.now().strftime("%Y-%m-%d--%H-%M-%S") - - new_fn = f"{date}_{get_commit(default='nocommit')[:8]}_{safe_fn(clean_path)}"[:MAX_TOMBSTONE_FN_LEN] - - crashlog_dir = os.path.join(ROOT, "crash") - mkdirs_exists_ok(crashlog_dir) - - shutil.copy(fn, os.path.join(crashlog_dir, new_fn)) - - def report_tombstone_apport(fn): f_size = os.path.getsize(fn) if f_size > MAX_SIZE: @@ -199,7 +158,7 @@ def main() -> NoReturn: if fn.endswith(".crash"): report_tombstone_apport(fn) else: - report_tombstone_android(fn) + cloudlog.error(f"unknown crash type: {fn}") except Exception: cloudlog.exception(f"Error reporting tombstone {fn}") From 6ce511cc605782c25d45c66b55f859caaf7ce516 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 7 Oct 2022 16:12:58 -0700 Subject: [PATCH 08/11] GM: use ECM brake-pressed threshold (#25970) * This brake position value disengages stock ACC, use it to avoid controls mismatch. 2016-2017 Volt will hit this threshold and disengage, must install new design of brake pedal retaining clip, TSB 16-NA-147. * 80 hz * comment * bump panda * update refs * bump panda * bump panda * bump panda * bump panda to master Co-authored-by: qadmus <42746943+qadmus@users.noreply.github.com> --- panda | 2 +- selfdrive/car/gm/carstate.py | 13 ++++++++----- selfdrive/test/process_replay/ref_commit | 2 +- 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/panda b/panda index 3334dc21f5..1303af2db2 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 3334dc21f5c55007c5a754dfd8ee5d642be3e2bb +Subproject commit 1303af2db29a72eee180b10c6097fa5b19c29207 diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 0bba1d29b8..f96a234dbd 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -40,9 +40,12 @@ class CarState(CarStateBase): else: ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL2"]["PRNDL2"], None)) - # Brake pedal's potentiometer returns near-zero reading even when pedal is not pressed. - ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] / 0xd0 - ret.brakePressed = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] >= 10 + # Some Volt 2016-17 have loose brake pedal push rod retainers which causes the ECM to believe + # that the brake is being intermittently pressed without user interaction. + # To avoid a cruise fault we need to match the ECM's brake pressed signal and threshold + # https://static.nhtsa.gov/odi/tsbs/2017/MC-10137629-9999.pdf + ret.brake = pt_cp.vl["ECMAcceleratorPos"]["BrakePedalPos"] / 0xd0 + ret.brakePressed = pt_cp.vl["ECMAcceleratorPos"]["BrakePedalPos"] >= 8 # Regen braking is braking if self.CP.transmissionType == TransmissionType.direct: @@ -100,7 +103,7 @@ class CarState(CarStateBase): def get_can_parser(CP): signals = [ # sig_name, sig_address - ("BrakePedalPosition", "EBCMBrakePedalPosition"), + ("BrakePedalPos", "ECMAcceleratorPos"), ("FrontLeftDoor", "BCMDoorBeltStatus"), ("FrontRightDoor", "BCMDoorBeltStatus"), ("RearLeftDoor", "BCMDoorBeltStatus"), @@ -141,7 +144,7 @@ class CarState(CarStateBase): ("ASCMSteeringButton", 33), ("ECMEngineStatus", 100), ("PSCMSteeringAngle", 100), - ("EBCMBrakePedalPosition", 100), + ("ECMAcceleratorPos", 80), ] if CP.transmissionType == TransmissionType.direct: diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 78367235e6..a89b4c2d71 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -f9536e41a6a160bdaa29d42bb164b0e4033857e5 +338c24158bb28952dcd1554ea91734b4281e2fed \ No newline at end of file From 6c5693e965b9c63f8678f52b9e9b5abe35f23feb Mon Sep 17 00:00:00 2001 From: YassineYousfi Date: Fri, 7 Oct 2022 17:22:42 -0700 Subject: [PATCH 09/11] faster rocket launcher model (#26009) * cache tokens 1456d261-d232-4654-8885-4d9fde883894/440 e63ab895-2222-4abd-a9a5-af86bb70e260/700 * udpate ref commit * bump tinygrad to master --- selfdrive/modeld/models/driving.h | 2 +- selfdrive/modeld/models/supercombo.onnx | 4 ++-- selfdrive/test/process_replay/model_replay_ref_commit | 2 +- tinygrad_repo | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index 90767384eb..8bb84d0245 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -16,7 +16,7 @@ #include "selfdrive/modeld/models/commonmodel.h" #include "selfdrive/modeld/runners/run.h" -constexpr int FEATURE_LEN = 2048; +constexpr int FEATURE_LEN = 128; constexpr int HISTORY_BUFFER_LEN = 99; constexpr int DESIRE_LEN = 8; constexpr int DESIRE_PRED_LEN = 4; diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index 37edc36c02..aee0ac37ff 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:30f30bc1251c03db135564ecbf7dc0bc96cbb07be0ebd3691edd8d555dc087fa -size 58539693 +oid sha256:c4d37af666344af6bb218e0b939b1152ad3784c15ac79e37bcf0124643c8286a +size 58539563 diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index 9727ef3d17..2446ec061d 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -008c0a622b7471c6234690d668f76bcb5dc8d999 +c171250d2cc013b3eca1cda4fb62f3d0dda28d4d diff --git a/tinygrad_repo b/tinygrad_repo index 2993dfe921..46f6db7522 160000 --- a/tinygrad_repo +++ b/tinygrad_repo @@ -1 +1 @@ -Subproject commit 2993dfe9219089bd1464741757f0c2ad67fe6a2e +Subproject commit 46f6db75228cb7744a3a0a87616bfffeffa93658 From 57a82ced285518cad129ec61814aab48bf51910e Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Fri, 7 Oct 2022 17:27:09 -0700 Subject: [PATCH 10/11] rawgpsd: publish ephemerides (#25931) * add svpoly parsing * Publish poly * add source check * add safety check for invalid gpsWeek values * address PR comments * add qcom ephemeris source type * bump cereal and laika Co-authored-by: Kurt Nistelberger --- cereal | 2 +- laika_repo | 2 +- selfdrive/locationd/laikad.py | 52 +++++++++++++++++++++++------ selfdrive/sensord/rawgps/rawgpsd.py | 30 ++++++++++++++--- selfdrive/sensord/rawgps/structs.py | 41 +++++++++++++++++++++++ 5 files changed, 111 insertions(+), 16 deletions(-) diff --git a/cereal b/cereal index 5ba96b6ded..b29717c4c3 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 5ba96b6ded57bcd91e60140ce0036f61370f8512 +Subproject commit b29717c4c328d5cf34d46f682f25267150f82637 diff --git a/laika_repo b/laika_repo index c8bc1fa01b..e1049cde0a 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit c8bc1fa01be9f22592efb991ee52d3d965d21968 +Subproject commit e1049cde0a68f7d4a70b1ebd76befdc0e163ad55 diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index c4fdfb9871..2769f394c5 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -16,7 +16,7 @@ from common.params import Params, put_nonblocking from laika import AstroDog from laika.constants import SECS_IN_HR, SECS_IN_MIN from laika.downloader import DownloadFailed -from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_ephem +from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_ephem, parse_qcom_ephem from laika.gps_time import GPSTime from laika.helpers import ConstellationId from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom @@ -61,6 +61,7 @@ class Laikad: self.last_pos_fix = [] self.last_pos_residual = [] self.last_pos_fix_t = None + self.gps_week = None self.use_qcom = use_qcom def load_cache(self): @@ -107,11 +108,11 @@ class Laikad: return self.last_pos_fix def is_good_report(self, gnss_msg): - if gnss_msg.which == 'drMeasurementReport' and self.use_qcom: + if gnss_msg.which() == 'drMeasurementReport' and self.use_qcom: constellation_id = ConstellationId.from_qcom_source(gnss_msg.drMeasurementReport.source) # TODO support GLONASS return constellation_id in [ConstellationId.GPS, ConstellationId.SBAS] - elif gnss_msg.which == 'measurementReport' and not self.use_qcom: + elif gnss_msg.which() == 'measurementReport' and not self.use_qcom: return True else: return False @@ -129,9 +130,28 @@ class Laikad: new_meas = read_raw_ublox(report) return week, tow, new_meas + def is_ephemeris(self, gnss_msg): + if self.use_qcom: + return gnss_msg.which() == 'drSvPoly' + else: + return gnss_msg.which() == 'ephemeris' + + def read_ephemeris(self, gnss_msg): + # TODO this only works on GLONASS + if self.use_qcom: + # TODO this is not robust to gps week rollover + if self.gps_week is None: + return + ephem = parse_qcom_ephem(gnss_msg.drSvPoly, self.gps_week) + else: + ephem = convert_ublox_ephem(gnss_msg.ephemeris) + self.astro_dog.add_navs({ephem.prn: [ephem]}) + self.cache_ephemeris(t=ephem.epoch) + def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False): if self.is_good_report(gnss_msg): week, tow, new_meas = self.read_report(gnss_msg) + self.gps_week = week t = gnss_mono_time * 1e-9 if week > 0: @@ -172,12 +192,10 @@ class Laikad: "correctedMeasurements": meas_msgs } return dat - # TODO this only works on GLONASS, qcom needs live ephemeris parsing too - elif gnss_msg.which == 'ephemeris': - ephem = convert_ublox_ephem(gnss_msg.ephemeris) - self.astro_dog.add_navs({ephem.prn: [ephem]}) - self.cache_ephemeris(t=ephem.epoch) - #elif gnss_msg.which == 'ionoData': + elif self.is_ephemeris(gnss_msg): + self.read_ephemeris(gnss_msg) + + #elif gnss_msg.which() == 'ionoData': # todo add this. Needed to better correct messages offline. First fix ublox_msg.cc to sent them. def update_localizer(self, est_pos, t: float, measurements: List[GNSSMeasurement]): @@ -265,9 +283,11 @@ def create_measurement_msg(meas: GNSSMeasurement): c.satVel = meas.sat_vel.tolist() ephem = meas.sat_ephemeris assert ephem is not None + week, time_of_week = -1, -1 if ephem.eph_type == EphemerisType.NAV: source_type = EphemerisSourceType.nav - week, time_of_week = -1, -1 + elif ephem.eph_type == EphemerisType.QCOM_POLY: + source_type = EphemerisSourceType.qcom else: assert ephem.file_epoch is not None week = ephem.file_epoch.week @@ -325,6 +345,7 @@ class EphemerisSourceType(IntEnum): nav = 0 nasaUltraRapid = 1 glonassIacUltraRapid = 2 + qcom = 3 def main(sm=None, pm=None): @@ -348,6 +369,17 @@ def main(sm=None, pm=None): if sm.updated[raw_gnss_socket]: gnss_msg = sm[raw_gnss_socket] + + # TODO: Understand and use remaining unknown constellations + if gnss_msg.which() == "drMeasurementReport": + if getattr(gnss_msg, gnss_msg.which()).source not in ['glonass', 'gps', 'beidou', 'sbas']: + continue + + if getattr(gnss_msg, gnss_msg.which()).gpsWeek > np.iinfo(np.int16).max: + # gpsWeek 65535 is received rarely from quectel, this cannot be + # passed to GnssMeasurements's gpsWeek (Int16) + continue + msg = laikad.process_gnss_msg(gnss_msg, sm.logMonoTime[raw_gnss_socket], block=replay) if msg is not None: pm.send('gnssMeasurements', msg) diff --git a/selfdrive/sensord/rawgps/rawgpsd.py b/selfdrive/sensord/rawgps/rawgpsd.py index c430acc5e5..c68e7aff99 100755 --- a/selfdrive/sensord/rawgps/rawgpsd.py +++ b/selfdrive/sensord/rawgps/rawgpsd.py @@ -14,12 +14,13 @@ import cereal.messaging as messaging from laika.gps_time import GPSTime 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, position_report, +from selfdrive.sensord.rawgps.structs import (dict_unpacker, position_report, relist, gps_measurement_report, gps_measurement_report_sv, glonass_measurement_report, glonass_measurement_report_sv, - oemdre_measurement_report, oemdre_measurement_report_sv, + oemdre_measurement_report, oemdre_measurement_report_sv, oemdre_svpoly_report, LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT, - LOG_GNSS_POSITION_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT) + LOG_GNSS_POSITION_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT, + LOG_GNSS_OEMDRE_SVPOLY_REPORT) DEBUG = int(os.getenv("DEBUG", "0"))==1 @@ -28,6 +29,7 @@ LOG_TYPES = [ LOG_GNSS_GLONASS_MEASUREMENT_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT, LOG_GNSS_POSITION_REPORT, + LOG_GNSS_OEMDRE_SVPOLY_REPORT, ] @@ -146,6 +148,9 @@ def main() -> NoReturn: unpack_oemdre_meas, size_oemdre_meas = dict_unpacker(oemdre_measurement_report, True) unpack_oemdre_meas_sv, size_oemdre_meas_sv = dict_unpacker(oemdre_measurement_report_sv, True) + unpack_svpoly, _ = dict_unpacker(oemdre_svpoly_report, True) + unpack_position, _ = dict_unpacker(position_report) + unpack_position, _ = dict_unpacker(position_report) # wait for ModemManager to come up @@ -258,7 +263,24 @@ def main() -> NoReturn: pm.send('gpsLocation', msg) - if log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]: + elif log_type == LOG_GNSS_OEMDRE_SVPOLY_REPORT: + msg = messaging.new_message('qcomGnss') + dat = unpack_svpoly(log_payload) + dat = relist(dat) + gnss = msg.qcomGnss + gnss.logTs = log_time + gnss.init('drSvPoly') + poly = gnss.drSvPoly + for k,v in dat.items(): + if k == "version": + assert v == 2 + elif k == "flags": + pass + else: + setattr(poly, k, v) + pm.send('qcomGnss', msg) + + elif log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]: msg = messaging.new_message('qcomGnss') gnss = msg.qcomGnss diff --git a/selfdrive/sensord/rawgps/structs.py b/selfdrive/sensord/rawgps/structs.py index 4bc9eca875..97e3d3d605 100644 --- a/selfdrive/sensord/rawgps/structs.py +++ b/selfdrive/sensord/rawgps/structs.py @@ -56,6 +56,29 @@ oemdre_measurement_report = """ uint8_t source; """ +oemdre_svpoly_report = """ + uint8_t version; + uint16_t sv_id; + int8_t frequency_index; + uint8_t flags; + uint16_t iode; + double t0; + double xyz0[3]; + double xyzN[9]; + float other[4]; + float position_uncertainty; + float iono_delay; + float iono_dot; + float sbas_iono_delay; + float sbas_iono_dot; + float tropo_delay; + float elevation; + float elevation_dot; + float elevation_uncertainty; + double velocity_coeff[12]; +""" + + oemdre_measurement_report_sv = """ uint8_t sv_id; uint8_t unkn; @@ -311,3 +334,21 @@ def dict_unpacker(ss, camelcase = False): nams = [name_to_camelcase(x) for x in nams] sz = calcsize(st) return lambda x: dict(zip(nams, unpack_from(st, x))), sz + +def relist(dat): + list_keys = set() + for key in dat.keys(): + if '[' in key: + list_keys.add(key.split('[')[0]) + list_dict = {} + for list_key in list_keys: + list_dict[list_key] = [] + i = 0 + while True: + key = list_key + f'[{i}]' + if key not in dat: + break + list_dict[list_key].append(dat[key]) + del dat[key] + i += 1 + return {**dat, **list_dict} From fb074378194db28067dbb77e8cd15db6ab5fd882 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Fri, 7 Oct 2022 19:15:04 -0700 Subject: [PATCH 11/11] Increase low speed jerk cost (#26008) * Increase low speed jerk cost * Update planner weight * Update ref_commit * Update lateral_planner.py * cleanup and refactor * Update ref_commit --- .../controls/lib/lateral_mpc_lib/lat_mpc.py | 16 ++++----- selfdrive/controls/lib/lateral_planner.py | 35 +++++++++++-------- selfdrive/test/process_replay/ref_commit | 2 +- 3 files changed, 30 insertions(+), 23 deletions(-) diff --git a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py index 07efad73c9..0cbd576341 100755 --- a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py +++ b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py @@ -20,6 +20,7 @@ P_DIM = 2 N = 16 COST_E_DIM = 3 COST_DIM = COST_E_DIM + 1 +SPEED_OFFSET = 10.0 MODEL_NAME = 'lat' ACADOS_SOLVER_TYPE = 'SQP_RTI' @@ -88,14 +89,13 @@ def gen_lat_ocp(): ocp.cost.yref = np.zeros((COST_DIM, )) ocp.cost.yref_e = np.zeros((COST_E_DIM, )) - # TODO hacky weights to keep behavior the same ocp.model.cost_y_expr = vertcat(y_ego, - ((v_ego + 5.0) * psi_ego), - ((v_ego + 5.0) * psi_rate_ego), - ((v_ego + 5.0) * psi_rate_ego_dot)) + ((v_ego + SPEED_OFFSET) * psi_ego), + ((v_ego + SPEED_OFFSET) * psi_rate_ego), + ((v_ego + SPEED_OFFSET) * psi_rate_ego_dot)) ocp.model.cost_y_expr_e = vertcat(y_ego, - ((v_ego + 5.0) * psi_ego), - ((v_ego + 5.0) * psi_rate_ego)) + ((v_ego + SPEED_OFFSET) * psi_ego), + ((v_ego + SPEED_OFFSET) * psi_rate_ego)) # set constraints ocp.constraints.constr_type = 'BGH' @@ -158,8 +158,8 @@ class LateralMpc(): self.yref[:,0] = y_pts v_ego = p_cp[0] # rotation_radius = p_cp[1] - self.yref[:,1] = heading_pts * (v_ego+5.0) - self.yref[:,2] = yaw_rate_pts * (v_ego+5.0) + self.yref[:,1] = heading_pts * (v_ego + SPEED_OFFSET) + self.yref[:,2] = yaw_rate_pts * (v_ego + SPEED_OFFSET) for i in range(N): self.solver.cost_set(i, "yref", self.yref[i]) self.solver.set(i, "p", p_cp) diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 10e3edbebe..d4c832b53b 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -12,6 +12,15 @@ from cereal import log TRAJECTORY_SIZE = 33 CAMERA_OFFSET = 0.04 + +PATH_COST = 1.0 +LATERAL_MOTION_COST = 0.11 +LATERAL_ACCEL_COST = 0.0 +LATERAL_JERK_COST = 0.05 + +MIN_SPEED = 1.5 + + class LateralPlanner: def __init__(self, CP): self.DH = DesireHelper() @@ -36,7 +45,8 @@ class LateralPlanner: self.lat_mpc.reset(x0=self.x0) def update(self, sm): - v_ego = sm['carState'].vEgo + # clip speed , lateral planning is not possible at 0 speed + self.v_ego = max(MIN_SPEED, sm['carState'].vEgo) measured_curvature = sm['controlsState'].curvature # Parse model predictions @@ -56,20 +66,19 @@ class LateralPlanner: self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob) 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], [1.0, 0.15]) - self.lat_mpc.set_weights(1.0, heading_cost, 0.0, .075) + self.lat_mpc.set_weights(PATH_COST, LATERAL_MOTION_COST, + LATERAL_ACCEL_COST, LATERAL_JERK_COST) - 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) - yaw_rate_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw_rate) + y_pts = np.interp(self.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(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw) + yaw_rate_pts = np.interp(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw_rate) self.y_pts = y_pts assert len(y_pts) == LAT_MPC_N + 1 assert len(heading_pts) == LAT_MPC_N + 1 assert len(yaw_rate_pts) == LAT_MPC_N + 1 - lateral_factor = max(0, self.factor1 - (self.factor2 * v_ego**2)) - p = np.array([v_ego, lateral_factor]) + lateral_factor = max(0, self.factor1 - (self.factor2 * self.v_ego**2)) + p = np.array([self.v_ego, lateral_factor]) self.lat_mpc.run(self.x0, p, y_pts, @@ -86,7 +95,7 @@ class LateralPlanner: t = sec_since_boot() if mpc_nans or self.lat_mpc.solution_status != 0: self.reset_mpc() - self.x0[3] = measured_curvature + self.x0[3] = measured_curvature * self.v_ego if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning("Lateral mpc - nan: True") @@ -106,10 +115,8 @@ class LateralPlanner: lateralPlan.dPathPoints = self.y_pts.tolist() lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist() - # clip speed for curv calculation at 1m/s, to prevent low speed extremes - clipped_speed = max(1.0, sm['carState'].vEgo) - lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3]/clipped_speed).tolist() - lateralPlan.curvatureRates = [float(x/clipped_speed) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0] + lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3]/self.v_ego).tolist() + lateralPlan.curvatureRates = [float(x/self.v_ego) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0] lateralPlan.mpcSolutionValid = bool(plan_solution_valid) lateralPlan.solverExecutionTime = self.lat_mpc.solve_time diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index a89b4c2d71..a4a24ca603 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -338c24158bb28952dcd1554ea91734b4281e2fed \ No newline at end of file +f636c68e2b4ed88d3731930cf15b6dee984eb6dd