From ce74bd41431e6bcc76ccf3de0c2edb69bcde4d48 Mon Sep 17 00:00:00 2001 From: David Peterson Date: Tue, 19 Apr 2022 16:25:56 -0400 Subject: [PATCH 01/16] docs: add video link for 2019 Subaru Crosstrek (#24251) * Update values.py Add video_link : SubaruCarInfo("Subaru Crosstrek 2018-19", good_torque=True),video_link="https://youtu.be/Agww7oE1k-s"), * fix syntax * timestamp to start of openpilot use Co-authored-by: Shane Smiskol --- selfdrive/car/subaru/values.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 1c524747d..76f7e8966 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -40,7 +40,7 @@ CAR_INFO: Dict[str, Union[SubaruCarInfo, List[SubaruCarInfo]]] = { CAR.ASCENT: SubaruCarInfo("Subaru Ascent 2019-20"), CAR.IMPREZA: [ SubaruCarInfo("Subaru Impreza 2017-19", good_torque=True), - SubaruCarInfo("Subaru Crosstrek 2018-19", good_torque=True), + SubaruCarInfo("Subaru Crosstrek 2018-19", video_link="https://youtu.be/Agww7oE1k-s?t=26", good_torque=True), ], CAR.IMPREZA_2020: [ SubaruCarInfo("Subaru Impreza 2020-21"), From 28149eae4d130e8feb4bf106f52b5a6f991890e8 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Tue, 19 Apr 2022 13:41:36 -0700 Subject: [PATCH 02/16] test_onroad: fix min -> max (#23824) * test_onroad: fix min -> max * raise that Co-authored-by: Comma Device --- selfdrive/test/test_onroad.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index a5fefcb6d..abbd25bb9 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -190,7 +190,7 @@ class TestOnroad(unittest.TestCase): cfgs = [("lateralPlan", 0.05, 0.05), ("longitudinalPlan", 0.05, 0.05)] for (s, instant_max, avg_max) in cfgs: ts = [getattr(getattr(m, s), "solverExecutionTime") for m in self.lr if m.which() == s] - self.assertLess(min(ts), instant_max, f"high '{s}' execution time: {min(ts)}") + self.assertLess(max(ts), instant_max, f"high '{s}' execution time: {max(ts)}") self.assertLess(np.mean(ts), avg_max, f"high avg '{s}' execution time: {np.mean(ts)}") result += f"'{s}' execution time: min {min(ts):.5f}s\n" result += f"'{s}' execution time: max {max(ts):.5f}s\n" @@ -206,13 +206,14 @@ class TestOnroad(unittest.TestCase): # TODO: this went up when plannerd cpu usage increased, why? cfgs = [ ("modelV2", 0.038, 0.036), - ("driverState", 0.028, 0.026), + ("driverState", 0.035, 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] - self.assertLess(min(ts), instant_max, f"high '{s}' execution time: {min(ts)}") + self.assertLess(max(ts), instant_max, f"high '{s}' execution time: {max(ts)}") self.assertLess(np.mean(ts), avg_max, f"high avg '{s}' execution time: {np.mean(ts)}") result += f"'{s}' execution time: min {min(ts):.5f}s\n" + result += f"'{s}' execution time: max {max(ts):.5f}s\n" result += f"'{s}' execution time: mean {np.mean(ts):.5f}s\n" result += "------------------------------------------------\n" print(result) From 76a83381977f9dd3d5b223e0dfb8e1617e2c7237 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Tue, 19 Apr 2022 14:13:05 -0700 Subject: [PATCH 03/16] manager: start bridge and web joystick for notcars (#24212) * manager: start bridge and web joystick for notcars * fix that * bump cereal Co-authored-by: Comma Device --- cereal | 2 +- selfdrive/manager/manager.py | 5 ++--- selfdrive/manager/process.py | 15 ++++++++++++--- selfdrive/manager/process_config.py | 5 ++++- tools/joystick/web.py | 13 ++++++++----- 5 files changed, 27 insertions(+), 13 deletions(-) diff --git a/cereal b/cereal index 1b342ce4e..6eb3994da 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 1b342ce4e00cacc0c87f3339cd7696e2ab0e62fe +Subproject commit 6eb3994daa0f64f57a36606a7bdd982ed2ac9d2d diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index b5fa43967..fca502029 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -131,16 +131,15 @@ def manager_thread() -> None: ensure_running(managed_processes.values(), started=False, not_run=ignore) started_prev = False - sm = messaging.SubMaster(['deviceState']) + sm = messaging.SubMaster(['deviceState', 'carParams'], poll=['deviceState']) pm = messaging.PubMaster(['managerState']) while True: sm.update() - not_run = ignore[:] started = sm['deviceState'].started driverview = params.get_bool("IsDriverViewEnabled") - ensure_running(managed_processes.values(), started, driverview, not_run) + ensure_running(managed_processes.values(), started=started, driverview=driverview, notcar=sm['carParams'].notCar, not_run=ignore) # trigger an update after going offroad if started_prev and not started and 'updated' in managed_processes: diff --git a/selfdrive/manager/process.py b/selfdrive/manager/process.py index 4cd9783b2..8551e96f3 100644 --- a/selfdrive/manager/process.py +++ b/selfdrive/manager/process.py @@ -71,6 +71,7 @@ class ManagerProcess(ABC): sigkill = False persistent = False driverview = False + notcar = False proc: Optional[Process] = None enabled = True name = "" @@ -182,13 +183,14 @@ class ManagerProcess(ABC): class NativeProcess(ManagerProcess): - def __init__(self, name, cwd, cmdline, enabled=True, persistent=False, driverview=False, unkillable=False, sigkill=False, watchdog_max_dt=None): + def __init__(self, name, cwd, cmdline, enabled=True, persistent=False, driverview=False, notcar=False, unkillable=False, sigkill=False, watchdog_max_dt=None): self.name = name self.cwd = cwd self.cmdline = cmdline self.enabled = enabled self.persistent = persistent self.driverview = driverview + self.notcar = notcar self.unkillable = unkillable self.sigkill = sigkill self.watchdog_max_dt = watchdog_max_dt @@ -213,12 +215,13 @@ class NativeProcess(ManagerProcess): class PythonProcess(ManagerProcess): - def __init__(self, name, module, enabled=True, persistent=False, driverview=False, unkillable=False, sigkill=False, watchdog_max_dt=None): + def __init__(self, name, module, enabled=True, persistent=False, driverview=False, notcar=False, unkillable=False, sigkill=False, watchdog_max_dt=None): self.name = name self.module = module self.enabled = enabled self.persistent = persistent self.driverview = driverview + self.notcar = notcar self.unkillable = unkillable self.sigkill = sigkill self.watchdog_max_dt = watchdog_max_dt @@ -284,7 +287,8 @@ class DaemonProcess(ManagerProcess): pass -def ensure_running(procs: ValuesView[ManagerProcess], started: bool, driverview: bool=False, not_run: Optional[List[str]]=None) -> None: +def ensure_running(procs: ValuesView[ManagerProcess], started: bool, driverview: bool=False, notcar: bool=False, + not_run: Optional[List[str]]=None) -> None: if not_run is None: not_run = [] @@ -297,6 +301,11 @@ def ensure_running(procs: ValuesView[ManagerProcess], started: bool, driverview: p.start() elif p.driverview and driverview: p.start() + elif p.notcar: + if notcar: + p.start() + else: + p.stop(block=False) elif started: p.start() else: diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 63790058f..43b22036d 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -14,7 +14,7 @@ procs = [ NativeProcess("logcatd", "selfdrive/logcatd", ["./logcatd"]), NativeProcess("loggerd", "selfdrive/loggerd", ["./loggerd"]), NativeProcess("modeld", "selfdrive/modeld", ["./modeld"]), - NativeProcess("navd", "selfdrive/ui/navd", ["./navd"], enabled=(PC or TICI), persistent=True), + NativeProcess("navd", "selfdrive/ui/navd", ["./navd"], persistent=True), NativeProcess("proclogd", "selfdrive/proclogd", ["./proclogd"]), NativeProcess("sensord", "selfdrive/sensord", ["./sensord"], enabled=not PC), NativeProcess("ubloxd", "selfdrive/locationd", ["./ubloxd"], enabled=(not PC or WEBCAM)), @@ -38,6 +38,9 @@ procs = [ PythonProcess("uploader", "selfdrive.loggerd.uploader", persistent=True), PythonProcess("statsd", "selfdrive.statsd", persistent=True), + NativeProcess("bridge", "cereal/messaging", ["./bridge"], notcar=True), + PythonProcess("webjoystick", "tools.joystick.web", notcar=True), + # Experimental PythonProcess("rawgpsd", "selfdrive.sensord.rawgps.rawgpsd", enabled=os.path.isfile("/persist/comma/use-quectel-rawgps")), ] diff --git a/tools/joystick/web.py b/tools/joystick/web.py index 403041362..143ca115c 100755 --- a/tools/joystick/web.py +++ b/tools/joystick/web.py @@ -1,7 +1,8 @@ #!/usr/bin/env python3 -from flask import Flask import time import threading +from flask import Flask + import cereal.messaging as messaging app = Flask(__name__) @@ -31,7 +32,7 @@ setInterval(function(){ def hello_world(): return index -last_send_time = time.time() +last_send_time = time.monotonic() @app.route("/control//") def control(x, y): global last_send_time @@ -42,12 +43,12 @@ def control(x, y): dat.testJoystick.axes = [y,x] dat.testJoystick.buttons = [False] pm.send('testJoystick', dat) - last_send_time = time.time() + last_send_time = time.monotonic() return "" def handle_timeout(): while 1: - this_time = time.time() + this_time = time.monotonic() if (last_send_time+0.5) < this_time: print("timeout, no web in %.2f s" % (this_time-last_send_time)) dat = messaging.new_message('testJoystick') @@ -56,7 +57,9 @@ def handle_timeout(): pm.send('testJoystick', dat) time.sleep(0.1) -if __name__ == '__main__': +def main(): threading.Thread(target=handle_timeout, daemon=True).start() app.run(host="0.0.0.0") +if __name__ == '__main__': + main() From 1ddcba2ca3a12a3b5a5092c098ed26ab49342e61 Mon Sep 17 00:00:00 2001 From: Erich Moraga <33645296+ErichMoraga@users.noreply.github.com> Date: Tue, 19 Apr 2022 16:26:42 -0500 Subject: [PATCH 04/16] Add missing AVALONH_TSS2 fwdCamera f/w (#24261) `@Hansal#7177` 2022 Avalon Hybrid Limited DongleID/route 16a02782fd3d706e|2022-04-18--23-48-48 --- 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 da4161195..e080f9ba0 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -290,6 +290,7 @@ FW_VERSIONS = { ], (Ecu.fwdCamera, 0x750, 0x6d): [ b'\x028646F4104100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', + b'\x028646F4104100\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', ], }, CAR.CAMRY: { From 397bd25e971abf5c32276cbd0c48c61d7b1eae30 Mon Sep 17 00:00:00 2001 From: Lukas Petersson Date: Tue, 19 Apr 2022 14:29:24 -0700 Subject: [PATCH 05/16] Disambiguate frame id in latencylogger (#24164) * add tools to disambiguate frame id * Update selfdrive/camerad/cameras/camera_qcom2.cc Co-authored-by: Gijs Koning * bug fix * remove duplicate code * PR comments * bug fix Co-authored-by: Gijs Koning Co-authored-by: Comma Device --- selfdrive/camerad/cameras/camera_qcom2.cc | 4 +- selfdrive/common/swaglog.cc | 62 ++++++++----- selfdrive/common/swaglog.h | 18 ++-- tools/latencylogger/README.md | 103 ++++++++++++---------- tools/latencylogger/latency_logger.py | 7 +- 5 files changed, 115 insertions(+), 79 deletions(-) diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc index 291dd261d..8986351fc 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.cc +++ b/selfdrive/camerad/cameras/camera_qcom2.cc @@ -1092,10 +1092,10 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) { if ((c == &s->road_cam && env_send_road) || (c == &s->wide_road_cam && env_send_wide_road)) { framed.setImage(get_frame_image(b)); } - LOGT("%s: Image set", c == &s->road_cam ? "RoadCamera" : "WideRoadCamera"); + LOGT(c->buf.cur_frame_data.frame_id, "%s: Image set", c == &s->road_cam ? "RoadCamera" : "WideRoadCamera"); if (c == &s->road_cam) { framed.setTransform(b->yuv_transform.v); - LOGT("%s: Transformed", c == &s->road_cam ? "RoadCamera" : "WideRoadCamera"); + LOGT(c->buf.cur_frame_data.frame_id, "%s: Transformed", "RoadCamera"); } s->pm->send(c == &s->road_cam ? "roadCameraState" : "wideRoadCameraState", msg); diff --git a/selfdrive/common/swaglog.cc b/selfdrive/common/swaglog.cc index 1110855cd..30e087dc2 100644 --- a/selfdrive/common/swaglog.cc +++ b/selfdrive/common/swaglog.cc @@ -6,6 +6,7 @@ #include #include +#include #include #include @@ -62,6 +63,7 @@ class SwaglogState : public LogState { static SwaglogState s = {}; bool LOG_TIMESTAMPS = getenv("LOG_TIMESTAMPS"); +uint32_t NO_FRAME_ID = std::numeric_limits::max(); static void log(int levelnum, const char* filename, int lineno, const char* func, const char* msg, const std::string& log_s) { if (levelnum >= s.print_level) { @@ -70,14 +72,9 @@ static void log(int levelnum, const char* filename, int lineno, const char* func char levelnum_c = levelnum; zmq_send(s.sock, (levelnum_c + log_s).c_str(), log_s.length() + 1, ZMQ_NOBLOCK); } -static void cloudlog_common(int levelnum, bool is_timestamp, const char* filename, int lineno, const char* func, - const char* fmt, va_list args) { - char* msg_buf = nullptr; - int ret = vasprintf(&msg_buf, fmt, args); - if (ret <= 0 || !msg_buf) return; - +static void cloudlog_common(int levelnum, const char* filename, int lineno, const char* func, + char* msg_buf, json11::Json::object msg_j={}) { std::lock_guard lk(s.lock); - if (!s.initialized) s.initialize(); json11::Json::object log_j = json11::Json::object { @@ -88,17 +85,10 @@ static void cloudlog_common(int levelnum, bool is_timestamp, const char* filenam {"funcname", func}, {"created", seconds_since_epoch()} }; - - if (is_timestamp) { - json11::Json::object tspt_j = json11::Json::object { - {"timestamp", json11::Json::object{ - {"event", msg_buf}, - {"time", std::to_string(nanos_since_boot())}} - } - }; - log_j["msg"] = tspt_j; - } else { + if (msg_j.empty()) { log_j["msg"] = msg_buf; + } else { + log_j["msg"] = msg_j; } std::string log_s = ((json11::Json)log_j).dump(); @@ -107,18 +97,46 @@ static void cloudlog_common(int levelnum, bool is_timestamp, const char* filenam } void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func, - const char* fmt, ...){ + const char* fmt, ...) { va_list args; va_start(args, fmt); - cloudlog_common(levelnum, false, filename, lineno, func, fmt, args); + char* msg_buf = nullptr; + int ret = vasprintf(&msg_buf, fmt, args); va_end(args); + if (ret <= 0 || !msg_buf) return; + cloudlog_common(levelnum, filename, lineno, func, msg_buf); } -void cloudlog_t(int levelnum, const char* filename, int lineno, const char* func, - const char* fmt, ...){ +void cloudlog_t_common(int levelnum, const char* filename, int lineno, const char* func, + uint32_t frame_id, const char* fmt, va_list args) { if (!LOG_TIMESTAMPS) return; + char* msg_buf = nullptr; + int ret = vasprintf(&msg_buf, fmt, args); + if (ret <= 0 || !msg_buf) return; + json11::Json::object tspt_j = json11::Json::object{ + {"event", msg_buf}, + {"time", std::to_string(nanos_since_boot())} + }; + if (frame_id < NO_FRAME_ID) { + tspt_j["frame_id"] = std::to_string(frame_id); + } + tspt_j = json11::Json::object{{"timestamp", tspt_j}}; + cloudlog_common(levelnum, filename, lineno, func, msg_buf, tspt_j); +} + + +void cloudlog_te(int levelnum, const char* filename, int lineno, const char* func, + const char* fmt, ...) { + va_list args; + va_start(args, fmt); + cloudlog_t_common(levelnum, filename, lineno, func, NO_FRAME_ID, fmt, args); + va_end(args); +} +void cloudlog_te(int levelnum, const char* filename, int lineno, const char* func, + uint32_t frame_id, const char* fmt, ...) { va_list args; va_start(args, fmt); - cloudlog_common(levelnum, true, filename, lineno, func, fmt, args); + cloudlog_t_common(levelnum, filename, lineno, func, frame_id, fmt, args); va_end(args); } + diff --git a/selfdrive/common/swaglog.h b/selfdrive/common/swaglog.h index 197372488..edae9fdc3 100644 --- a/selfdrive/common/swaglog.h +++ b/selfdrive/common/swaglog.h @@ -12,16 +12,21 @@ void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func, const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/; -void cloudlog_t(int levelnum, const char* filename, int lineno, const char* func, - const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/; +void cloudlog_te(int levelnum, const char* filename, int lineno, const char* func, + const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/; + +void cloudlog_te(int levelnum, const char* filename, int lineno, const char* func, + uint32_t frame_id, const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/; + #define cloudlog(lvl, fmt, ...) cloudlog_e(lvl, __FILE__, __LINE__, \ __func__, \ fmt, ## __VA_ARGS__); -#define cloudlog_t_m(lvl, fmt, ...) cloudlog_t(lvl, __FILE__, __LINE__, \ - __func__, \ - fmt, ## __VA_ARGS__); +#define cloudlog_t(lvl, ...) cloudlog_te(lvl, __FILE__, __LINE__, \ + __func__, \ + __VA_ARGS__); + #define cloudlog_rl(burst, millis, lvl, fmt, ...) \ { \ @@ -52,7 +57,8 @@ void cloudlog_t(int levelnum, const char* filename, int lineno, const char* func } \ } -#define LOGT(fmt, ...) cloudlog_t_m(CLOUDLOG_DEBUG, fmt,## __VA_ARGS__) + +#define LOGT(...) cloudlog_t(CLOUDLOG_DEBUG, __VA_ARGS__) #define LOGD(fmt, ...) cloudlog(CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__) #define LOG(fmt, ...) cloudlog(CLOUDLOG_INFO, fmt, ## __VA_ARGS__) #define LOGW(fmt, ...) cloudlog(CLOUDLOG_WARNING, fmt, ## __VA_ARGS__) diff --git a/tools/latencylogger/README.md b/tools/latencylogger/README.md index 20de8212a..e6884cc48 100644 --- a/tools/latencylogger/README.md +++ b/tools/latencylogger/README.md @@ -20,6 +20,7 @@ optional arguments: --demo Use the demo route instead of providing one (default: False) --plot If a plot should be generated (default: False) ``` +To timestamp an event, use `LOGT("msg")` in c++ code or `cloudlog.timestamp("msg")` in python code. If the print is warning for frameId assignment ambiguity, use `LOGT(frameId ,"msg")`. ## Examples Plotting with relative starts each process at time=0 and gives a nice overview. Timestamps are visualized as diamonds. The opacity allows for visualization of overlapping services. @@ -31,58 +32,64 @@ Plotting without relative provides info about the frames relative time. Printed timestamps of a frame with internal durations. ``` -Frame ID: 303 +Frame ID: 371 camerad - roadCameraState start of frame 0.0 - wideRoadCameraState start of frame 0.091926 - RoadCamera: Image set 1.691696 - RoadCamera: Transformed 1.812841 - roadCameraState published 51.775466 - wideRoadCameraState published 54.935164 - roadCameraState.processingTime 1.6455530421808362 - wideRoadCameraState.processingTime 4.790564067661762 + wideRoadCameraState start of frame 0.0 + roadCameraState start of frame 0.072395 + wideRoadCameraState published 47.804745 + WideRoadCamera: Image set 47.839849 + roadCameraState published 48.319166 + RoadCamera: Image set 48.354478 + RoadCamera: Transformed 48.430258 + wideRoadCameraState.processingTime 16.733376309275627 + roadCameraState.processingTime 16.218071803450584 modeld - Image added 56.628788 - Extra image added 57.459923 - Execution finished 75.091306 - modelV2 published 75.24797 - modelV2.modelExecutionTime 20.00947669148445 + Image added 51.346522 + Extra image added 53.179467 + Execution finished 71.584437 + modelV2 published 71.76881 + modelV2.modelExecutionTime 22.54236489534378 modelV2.gpuExecutionTime 0.0 plannerd - lateralPlan published 80.426861 - longitudinalPlan published 85.722781 - lateralPlan.solverExecutionTime 1.0600379901006818 - longitudinalPlan.solverExecutionTime 1.3830000534653664 + lateralPlan published 77.381862 + longitudinalPlan published 84.207972 + lateralPlan.solverExecutionTime 1.3547739945352077 + longitudinalPlan.solverExecutionTime 2.0179999992251396 controlsd - Data sampled 89.436221 - Events updated 90.356522 - sendcan published 91.396092 - controlsState published 91.77843 - Data sampled 99.885876 - Events updated 100.696855 - sendcan published 101.600489 - controlsState published 101.941839 - Data sampled 110.087669 - Events updated 111.025365 - sendcan published 112.305921 - controlsState published 112.70451 - Data sampled 119.692803 - Events updated 120.56774 - sendcan published 121.735016 - controlsState published 122.142823 - Data sampled 129.264761 - Events updated 130.024282 - sendcan published 130.950364 - controlsState published 131.281558 + Data sampled 78.909759 + Events updated 79.711884 + sendcan published 80.721038 + controlsState published 81.081398 + Data sampled 88.663748 + Events updated 89.535403 + sendcan published 90.587889 + controlsState published 91.019707 + Data sampled 98.667003 + Events updated 99.661261 + sendcan published 100.776507 + controlsState published 101.198794 + Data sampled 108.967078 + Events updated 109.95842 + sendcan published 111.263142 + controlsState published 111.678085 + Data sampled 118.574923 + Events updated 119.608555 + sendcan published 120.73427 + controlsState published 121.111036 + Data sampled 128.596408 + Events updated 129.382283 + sendcan published 130.330083 + controlsState published 130.676485 boardd - sending sendcan to panda: 250027001751393037323631 101.705487 - sendcan sent to panda: 250027001751393037323631 102.042462 - sending sendcan to panda: 250027001751393037323631 112.416961 - sendcan sent to panda: 250027001751393037323631 112.792269 - sending sendcan to panda: 250027001751393037323631 121.850952 - sendcan sent to panda: 250027001751393037323631 122.231103 - sending sendcan to panda: 250027001751393037323631 131.045206 - sendcan sent to panda: 250027001751393037323631 131.351296 - sending sendcan to panda: 250027001751393037323631 141.340592 - sendcan sent to panda: 250027001751393037323631 141.700744 + sending sendcan to panda: 250027001751393037323631 90.7257 + sendcan sent to panda: 250027001751393037323631 91.078143 + sending sendcan to panda: 250027001751393037323631 100.941766 + sendcan sent to panda: 250027001751393037323631 101.306865 + sending sendcan to panda: 250027001751393037323631 111.411786 + sendcan sent to panda: 250027001751393037323631 111.754074 + sending sendcan to panda: 250027001751393037323631 120.875987 + sendcan sent to panda: 250027001751393037323631 121.188535 + sending sendcan to panda: 250027001751393037323631 130.454248 + sendcan sent to panda: 250027001751393037323631 130.757994 + sending sendcan to panda: 250027001751393037323631 140.353234 ``` diff --git a/tools/latencylogger/latency_logger.py b/tools/latencylogger/latency_logger.py index 2994df2c5..0ccb8f9b3 100644 --- a/tools/latencylogger/latency_logger.py +++ b/tools/latencylogger/latency_logger.py @@ -85,7 +85,6 @@ def read_logs(lr): assert len(frame_mismatches) < 20, "Too many frame mismatches" return (data, frame_mismatches) - def find_frame_id(time, service, start_times, end_times): for frame_id in reversed(start_times): if start_times[frame_id][service] and end_times[frame_id][service]: @@ -108,6 +107,10 @@ def insert_cloudlogs(lr, timestamps, start_times, end_times): # Filter out controlsd messages which arrive before the camera loop continue + if "frame_id" in jmsg['msg']['timestamp']: + timestamps[int(jmsg['msg']['timestamp']['frame_id'])][service].append((event, time)) + continue + if service == "boardd": timestamps[latest_controls_frameid][service].append((event, time)) end_times[latest_controls_frameid][service] = time @@ -117,6 +120,8 @@ def insert_cloudlogs(lr, timestamps, start_times, end_times): if frame_id: if service == 'controlsd': latest_controls_frameid = frame_id + if next(frame_id_gen, False): + event += " (warning: ambiguity)" timestamps[frame_id][service].append((event, time)) else: failed_inserts += 1 From 27b067446a084a92625427de4db6196498ffcba6 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Tue, 19 Apr 2022 15:50:04 -0700 Subject: [PATCH 06/16] modeld tests (#24263) --- .github/workflows/selfdrive_tests.yaml | 1 + selfdrive/modeld/modeld.cc | 12 ++--- selfdrive/modeld/test/test_modeld.py | 72 ++++++++++++++++++++++++++ 3 files changed, 77 insertions(+), 8 deletions(-) create mode 100755 selfdrive/modeld/test/test_modeld.py diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index 03dd4f468..0c04a0e9e 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -257,6 +257,7 @@ jobs: $UNIT_TEST selfdrive/athena && \ $UNIT_TEST selfdrive/thermald && \ $UNIT_TEST selfdrive/hardware/tici && \ + $UNIT_TEST selfdrive/modeld && \ $UNIT_TEST tools/lib/tests && \ ./selfdrive/boardd/tests/test_boardd_usbprotocol && \ ./selfdrive/common/tests/test_util && \ diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index dd0bafff3..639acd2d2 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -51,10 +51,6 @@ mat3 update_calibration(Eigen::Matrix &extrinsics, bool wide_camera return matmul3(yuv_transform, transform); } -static uint64_t get_ts(const VisionIpcBufExtra &extra) { - return Hardware::TICI() ? extra.timestamp_sof : extra.timestamp_eof; -} - void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcClient &vipc_client_extra, bool main_wide_camera, bool use_extra_client) { // messaging @@ -80,7 +76,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl while (!do_exit) { // Keep receiving frames until we are at least 1 frame ahead of previous extra frame - while (get_ts(meta_main) < get_ts(meta_extra) + 25000000ULL) { + while (meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000ULL) { buf_main = vipc_client_main.recv(&meta_main); if (buf_main == nullptr) break; } @@ -94,7 +90,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl // Keep receiving extra frames until frame id matches main camera do { buf_extra = vipc_client_extra.recv(&meta_extra); - } while (buf_extra != nullptr && get_ts(meta_main) > get_ts(meta_extra) + 25000000ULL); + } while (buf_extra != nullptr && meta_main.timestamp_sof > meta_extra.timestamp_sof + 25000000ULL); if (buf_extra == nullptr) { LOGE("vipc_client_extra no frame"); @@ -124,7 +120,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl } model_transform_main = update_calibration(extrinsic_matrix_eigen, main_wide_camera, false); - model_transform_extra = update_calibration(extrinsic_matrix_eigen, Hardware::TICI(), true); + model_transform_extra = update_calibration(extrinsic_matrix_eigen, true, true); live_calib_seen = true; } @@ -169,7 +165,7 @@ int main(int argc, char **argv) { } bool main_wide_camera = Hardware::TICI() ? Params().getBool("EnableWideCamera") : false; - bool use_extra_client = Hardware::TICI() && !main_wide_camera; + bool use_extra_client = !main_wide_camera; // cl init cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); diff --git a/selfdrive/modeld/test/test_modeld.py b/selfdrive/modeld/test/test_modeld.py new file mode 100755 index 000000000..93ad0f560 --- /dev/null +++ b/selfdrive/modeld/test/test_modeld.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python3 +import time +import unittest +import numpy as np + +import cereal.messaging as messaging +from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType # pylint: disable=no-name-in-module, import-error +from common.transformations.camera import tici_f_frame_size +from common.realtime import DT_MDL +from selfdrive.manager.process_config import managed_processes + + +VIPC_STREAM = {"roadCameraState": VisionStreamType.VISION_STREAM_ROAD, "driverCameraState": VisionStreamType.VISION_STREAM_DRIVER, + "wideRoadCameraState": VisionStreamType.VISION_STREAM_WIDE_ROAD} + +IMG = np.zeros(int(tici_f_frame_size[0]*tici_f_frame_size[1]*(3/2)), dtype=np.uint8) +IMG_BYTES = IMG.flatten().tobytes() + +class TestModeld(unittest.TestCase): + + def setUp(self): + self.vipc_server = VisionIpcServer("camerad") + self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, False, *tici_f_frame_size) + self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, False, *tici_f_frame_size) + self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, *tici_f_frame_size) + self.vipc_server.start_listener() + + self.sm = messaging.SubMaster(['modelV2', 'cameraOdometry']) + self.pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'driverCameraState', 'liveCalibration', 'lateralPlan']) + + managed_processes['modeld'].start() + time.sleep(0.2) + self.sm.update(1000) + + def tearDown(self): + managed_processes['modeld'].stop() + del self.vipc_server + + def test_modeld(self): + for n in range(1, 500): + for cam in ('roadCameraState', 'wideRoadCameraState'): + msg = messaging.new_message(cam) + cs = getattr(msg, cam) + cs.frameId = n + cs.timestampSof = int((n * DT_MDL) * 1e9) + cs.timestampEof = int(cs.timestampSof + (DT_MDL * 1e9)) + + self.pm.send(msg.which(), msg) + self.vipc_server.send(VIPC_STREAM[msg.which()], IMG_BYTES, cs.frameId, + cs.timestampSof, cs.timestampEof) + + self.sm.update(5000) + if self.sm['modelV2'].frameId != self.sm['cameraOdometry'].frameId: + self.sm.update(1000) + + mdl = self.sm['modelV2'] + self.assertEqual(mdl.frameId, n) + self.assertEqual(mdl.frameIdExtra, n) + self.assertEqual(mdl.timestampEof, cs.timestampEof) + self.assertEqual(mdl.frameAge, 0) + self.assertEqual(mdl.frameDropPerc, 0) + + odo = self.sm['cameraOdometry'] + self.assertEqual(odo.frameId, n) + self.assertEqual(odo.timestampEof, cs.timestampEof) + + def test_skipped_frames(self): + pass + + +if __name__ == "__main__": + unittest.main() From 273831aa14950b71ff84073d716a0fef2d1fa31e Mon Sep 17 00:00:00 2001 From: George Hotz Date: Tue, 19 Apr 2022 15:53:58 -0700 Subject: [PATCH 07/16] ui: make the path double wide for e2e --- selfdrive/ui/ui.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 583c03a5d..3a9cff2f7 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -98,7 +98,7 @@ static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) { max_distance = std::clamp((float)(lead_d - fmin(lead_d * 0.35, 10.)), 0.0f, max_distance); } max_idx = get_path_length_idx(model_position, max_distance); - update_line_data(s, model_position, 0.5, 1.22, &scene.track_vertices, max_idx); + update_line_data(s, model_position, scene.end_to_end ? 1.0 : 0.5, 1.22, &scene.track_vertices, max_idx); } static void update_sockets(UIState *s) { From 492489e55caceddd8e844874abe4662a6623a162 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 19 Apr 2022 16:07:46 -0700 Subject: [PATCH 08/16] ui: adjust e2e path width --- selfdrive/ui/ui.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 3a9cff2f7..e3de15119 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -98,7 +98,7 @@ static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) { max_distance = std::clamp((float)(lead_d - fmin(lead_d * 0.35, 10.)), 0.0f, max_distance); } max_idx = get_path_length_idx(model_position, max_distance); - update_line_data(s, model_position, scene.end_to_end ? 1.0 : 0.5, 1.22, &scene.track_vertices, max_idx); + update_line_data(s, model_position, scene.end_to_end ? 0.9 : 0.5, 1.22, &scene.track_vertices, max_idx); } static void update_sockets(UIState *s) { From 6877059b450b37c478042139fe847419e0c6bba8 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Tue, 19 Apr 2022 17:25:59 -0700 Subject: [PATCH 09/16] add joystick to release --- release/files_common | 1 + 1 file changed, 1 insertion(+) diff --git a/release/files_common b/release/files_common index 905f58f60..eb3d825b0 100644 --- a/release/files_common +++ b/release/files_common @@ -64,6 +64,7 @@ models/dmonitoring_model_q.dlc release/* tools/lib/* +tools/joystick/* selfdrive/version.py From fe0bcdaef6b1fd3fc9d855aefeba755b35dfc222 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Tue, 19 Apr 2022 19:34:31 -0700 Subject: [PATCH 10/16] Lateral torque-based control with roll on TSS2 corolla and TSSP rav4 (#24260) * Initial commit * Fix bugs * Need more torque rate * Cleanup cray cray control * Write nicely * Chiiil * Not relevant for cray cray control * Do some logging * Seems like it has more torque than I thought * Bit more feedforward * Tune change * Retune * Retune * Little more chill * Add coroll * Add corolla * Give craycray a good name * Update to proper logging * D to the PI * Should be in radians * Add d * Start oscillations * Add D term * Only change torque rate limits for new tune * Add d logging * Should be enough * Wrong sign in D * Downtune a little * Needed to prevent faults * Add lqr rav4 to tune * Try derivative again * Data based retune * Data based retune * add friction compensation * Doesnt need too much P with friction comp * remove lqr * Remove kd * Fix tests * fix tests * Too much error * Get roll induced error under 1cm/deg * Too much jitter * Do roll comp * Add ki * Final update * Update refs * Cleanup latcontrol_torque a little more --- release/files_common | 2 +- selfdrive/car/tests/test_car_interfaces.py | 4 +- selfdrive/car/tests/test_models.py | 4 +- selfdrive/car/toyota/carcontroller.py | 3 +- selfdrive/car/toyota/interface.py | 7 +- selfdrive/car/toyota/tunes.py | 24 +++--- selfdrive/car/toyota/values.py | 10 ++- selfdrive/controls/controlsd.py | 12 +-- selfdrive/controls/lib/latcontrol.py | 2 +- selfdrive/controls/lib/latcontrol_angle.py | 2 +- selfdrive/controls/lib/latcontrol_indi.py | 2 +- selfdrive/controls/lib/latcontrol_lqr.py | 84 ------------------- selfdrive/controls/lib/latcontrol_pid.py | 2 +- selfdrive/controls/lib/latcontrol_torque.py | 79 +++++++++++++++++ .../controls/lib/tests/test_latcontrol.py | 4 +- selfdrive/test/process_replay/ref_commit | 2 +- 16 files changed, 120 insertions(+), 123 deletions(-) delete mode 100644 selfdrive/controls/lib/latcontrol_lqr.py create mode 100644 selfdrive/controls/lib/latcontrol_torque.py diff --git a/release/files_common b/release/files_common index eb3d825b0..38301cafb 100644 --- a/release/files_common +++ b/release/files_common @@ -238,7 +238,7 @@ selfdrive/controls/lib/events.py selfdrive/controls/lib/lane_planner.py selfdrive/controls/lib/latcontrol_angle.py selfdrive/controls/lib/latcontrol_indi.py -selfdrive/controls/lib/latcontrol_lqr.py +selfdrive/controls/lib/latcontrol_torque.py selfdrive/controls/lib/latcontrol_pid.py selfdrive/controls/lib/latcontrol.py selfdrive/controls/lib/lateral_planner.py diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 2f4984c3d..d97b16e68 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -38,8 +38,8 @@ class TestCarInterfaces(unittest.TestCase): tuning = car_params.lateralTuning.which() if tuning == 'pid': self.assertTrue(len(car_params.lateralTuning.pid.kpV)) - elif tuning == 'lqr': - self.assertTrue(len(car_params.lateralTuning.lqr.a)) + elif tuning == 'torque': + self.assertTrue(car_params.lateralTuning.torque.kf > 0) elif tuning == 'indi': self.assertTrue(len(car_params.lateralTuning.indi.outerLoopGainV)) diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index 7e02fd2a0..b9c36db07 100755 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -115,8 +115,8 @@ class TestCarModel(unittest.TestCase): tuning = self.CP.lateralTuning.which() if tuning == 'pid': self.assertTrue(len(self.CP.lateralTuning.pid.kpV)) - elif tuning == 'lqr': - self.assertTrue(len(self.CP.lateralTuning.lqr.a)) + elif tuning == 'torque': + self.assertTrue(self.CP.lateralTuning.torque.kf > 0) elif tuning == 'indi': self.assertTrue(len(self.CP.lateralTuning.indi.outerLoopGainV)) else: diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index c0868f328..1c01703d7 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -14,6 +14,7 @@ VisualAlert = car.CarControl.HUDControl.VisualAlert class CarController: def __init__(self, dbc_name, CP, VM): self.CP = CP + self.torque_rate_limits = CarControllerParams(self.CP) self.frame = 0 self.last_steer = 0 self.alert_active = False @@ -50,7 +51,7 @@ class CarController: # steer torque new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX)) - apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, CarControllerParams) + apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.torque_rate_limits) self.steer_rate_limited = new_steer != apply_steer # Cut steering while we're in a known fault state (2s) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index f7d5aba7a..4b64b2eb2 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -34,7 +34,6 @@ 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 - set_lat_tune(ret.lateralTuning, LatTunes.INDI_PRIUS) ret.steerActuatorDelay = 0.3 @@ -44,7 +43,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 17.4 tire_stiffness_factor = 0.5533 ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG - set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4) + set_lat_tune(ret.lateralTuning, LatTunes.TORQUE) elif candidate in (CAR.RAV4, CAR.RAV4H): stop_and_go = True if (candidate in CAR.RAV4H) else False @@ -52,7 +51,7 @@ 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 - set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4) + set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_TORQUE=2.5, FRICTION=0.06) elif candidate == CAR.COROLLA: ret.wheelbase = 2.70 @@ -133,7 +132,7 @@ 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 - set_lat_tune(ret.lateralTuning, LatTunes.PID_D) + set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_TORQUE=3.0, FRICTION=0.08) elif candidate in (CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_ESH): stop_and_go = True diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py index f26fc72a0..81fd69582 100644 --- a/selfdrive/car/toyota/tunes.py +++ b/selfdrive/car/toyota/tunes.py @@ -24,6 +24,7 @@ class LatTunes(Enum): PID_L = 13 PID_M = 14 PID_N = 15 + TORQUE = 16 ###### LONG ###### @@ -49,8 +50,15 @@ def set_long_tune(tune, name): ###### LAT ###### -def set_lat_tune(tune, name): - if name == LatTunes.INDI_PRIUS: +def set_lat_tune(tune, name, MAX_TORQUE=2.5, FRICTION=.1): + if name == LatTunes.TORQUE: + tune.init('torque') + tune.torque.useSteeringAngle = True + tune.torque.kp = 2.0 / MAX_TORQUE + tune.torque.kf = 1.0 / MAX_TORQUE + tune.torque.ki = 0.5 / MAX_TORQUE + tune.torque.friction = FRICTION + elif name == LatTunes.INDI_PRIUS: tune.init('indi') tune.indi.innerLoopGainBP = [0.] tune.indi.innerLoopGainV = [4.0] @@ -60,18 +68,6 @@ def set_lat_tune(tune, name): tune.indi.timeConstantV = [1.0] tune.indi.actuatorEffectivenessBP = [0.] tune.indi.actuatorEffectivenessV = [1.0] - - elif name == LatTunes.LQR_RAV4: - tune.init('lqr') - tune.lqr.scale = 1500.0 - tune.lqr.ki = 0.05 - tune.lqr.a = [0., 1., -0.22619643, 1.21822268] - tune.lqr.b = [-1.92006585e-04, 3.95603032e-05] - tune.lqr.c = [1., 0.] - tune.lqr.k = [-110.73572306, 451.22718255] - tune.lqr.l = [0.3233671, 0.3185757] - tune.lqr.dcGain = 0.002237852961363602 - elif 'PID' in str(name): tune.init('pid') tune.pid.kiBP = [0.0] diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index e080f9ba0..cc5d9a581 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -18,10 +18,16 @@ class CarControllerParams: ACCEL_MIN = -3.5 # m/s2 STEER_MAX = 1500 - STEER_DELTA_UP = 10 # 1.5s time to peak torque - STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50) STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor + def __init__(self, CP): + if CP.lateralTuning.which == 'torque': + self.STEER_DELTA_UP = 15 # 1.0s time to peak torque + self.STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50) + else: + self.STEER_DELTA_UP = 10 # 1.5s time to peak torque + self.STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50) + class ToyotaFlags(IntFlag): HYBRID = 1 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index f098657fb..5bd75548f 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -20,8 +20,8 @@ from selfdrive.controls.lib.drive_helpers import get_lag_adjusted_curvature from selfdrive.controls.lib.longcontrol import LongControl from selfdrive.controls.lib.latcontrol_pid import LatControlPID from selfdrive.controls.lib.latcontrol_indi import LatControlINDI -from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR from selfdrive.controls.lib.latcontrol_angle import LatControlAngle +from selfdrive.controls.lib.latcontrol_torque import LatControlTorque 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 @@ -144,8 +144,8 @@ class Controls: self.LaC = LatControlPID(self.CP, self.CI) elif self.CP.lateralTuning.which() == 'indi': self.LaC = LatControlINDI(self.CP, self.CI) - elif self.CP.lateralTuning.which() == 'lqr': - self.LaC = LatControlLQR(self.CP, self.CI) + elif self.CP.lateralTuning.which() == 'torque': + self.LaC = LatControlTorque(self.CP, self.CI) self.initialized = False self.state = State.disabled @@ -566,7 +566,7 @@ class Controls: lat_plan.curvatureRates) actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.CP, self.VM, params, self.last_actuators, desired_curvature, - desired_curvature_rate) + desired_curvature_rate, self.sm['liveLocationKalman']) else: lac_log = log.ControlsState.LateralDebugState.new_message() if self.sm.rcv_frame['testJoystick'] > 0: @@ -730,8 +730,8 @@ class Controls: controlsState.lateralControlState.angleState = lac_log elif lat_tuning == 'pid': controlsState.lateralControlState.pidState = lac_log - elif lat_tuning == 'lqr': - controlsState.lateralControlState.lqrState = lac_log + elif lat_tuning == 'torque': + controlsState.lateralControlState.torqueState = lac_log elif lat_tuning == 'indi': controlsState.lateralControlState.indiState = lac_log diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 4a67715fc..0d137df73 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -16,7 +16,7 @@ class LatControl(ABC): self.steer_max = 1.0 @abstractmethod - def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): + def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): pass def reset(self): diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index c93535631..8d09432b6 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -7,7 +7,7 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees class LatControlAngle(LatControl): - def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): + def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): angle_log = log.ControlsState.LateralAngleState.new_message() if CS.vEgo < MIN_STEER_SPEED or not active: diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index 2db2d4388..4d2a72762 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -63,7 +63,7 @@ class LatControlINDI(LatControl): self.steer_filter.x = 0. self.speed = 0. - def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): + def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): self.speed = CS.vEgo # Update Kalman filter y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]]) diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py deleted file mode 100644 index 583244882..000000000 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ /dev/null @@ -1,84 +0,0 @@ -import math -import numpy as np - -from common.numpy_fast import clip -from common.realtime import DT_CTRL -from cereal import log -from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED - - -class LatControlLQR(LatControl): - def __init__(self, CP, CI): - super().__init__(CP, CI) - self.scale = CP.lateralTuning.lqr.scale - self.ki = CP.lateralTuning.lqr.ki - - self.A = np.array(CP.lateralTuning.lqr.a).reshape((2, 2)) - self.B = np.array(CP.lateralTuning.lqr.b).reshape((2, 1)) - self.C = np.array(CP.lateralTuning.lqr.c).reshape((1, 2)) - self.K = np.array(CP.lateralTuning.lqr.k).reshape((1, 2)) - self.L = np.array(CP.lateralTuning.lqr.l).reshape((2, 1)) - self.dc_gain = CP.lateralTuning.lqr.dcGain - - self.x_hat = np.array([[0], [0]]) - self.i_unwind_rate = 0.3 * DT_CTRL - self.i_rate = 1.0 * DT_CTRL - - self.reset() - - def reset(self): - super().reset() - self.i_lqr = 0.0 - - def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): - lqr_log = log.ControlsState.LateralLQRState.new_message() - - torque_scale = (0.45 + CS.vEgo / 60.0)**2 # Scale actuator model with speed - - # Subtract offset. Zero angle should correspond to zero torque - steering_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetAverageDeg - - desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) - - instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg - desired_angle += instant_offset # Only add offset that originates from vehicle model errors - lqr_log.steeringAngleDesiredDeg = desired_angle - - # Update Kalman filter - angle_steers_k = float(self.C.dot(self.x_hat)) - e = steering_angle_no_offset - angle_steers_k - self.x_hat = self.A.dot(self.x_hat) + self.B.dot(CS.steeringTorqueEps / torque_scale) + self.L.dot(e) - - if CS.vEgo < MIN_STEER_SPEED or not active: - lqr_log.active = False - lqr_output = 0. - output_steer = 0. - self.reset() - else: - lqr_log.active = True - - # LQR - u_lqr = float(desired_angle / self.dc_gain - self.K.dot(self.x_hat)) - lqr_output = torque_scale * u_lqr / self.scale - - # Integrator - if CS.steeringPressed: - self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr)) - else: - error = desired_angle - angle_steers_k - i = self.i_lqr + self.ki * self.i_rate * error - control = lqr_output + i - - if (error >= 0 and (control <= self.steer_max or i < 0.0)) or \ - (error <= 0 and (control >= -self.steer_max or i > 0.0)): - self.i_lqr = i - - output_steer = lqr_output + self.i_lqr - output_steer = clip(output_steer, -self.steer_max, self.steer_max) - - lqr_log.steeringAngleDeg = angle_steers_k - lqr_log.i = self.i_lqr - lqr_log.output = output_steer - lqr_log.lqrOutput = lqr_output - lqr_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS) - return output_steer, desired_angle, lqr_log diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 67d17e088..813ba13ab 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -17,7 +17,7 @@ class LatControlPID(LatControl): super().reset() self.pid.reset() - def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): + def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py new file mode 100644 index 000000000..2816b2014 --- /dev/null +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -0,0 +1,79 @@ +import math +from selfdrive.controls.lib.pid import PIDController +from common.numpy_fast import interp +from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED +from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY +from cereal import log + +# At higher speeds (25+mph) we can assume: +# Lateral acceleration achieved by a specific car correlates to +# torque applied to the steering rack. It does not correlate to +# wheel slip, or to speed. + +# This controller applies torque to achieve desired lateral +# accelerations. To compensate for the low speed effects we +# use a LOW_SPEED_FACTOR in the error. Additionally there is +# friction in the steering wheel that needs to be overcome to +# move it at all, this is compensated for too. + + +LOW_SPEED_FACTOR = 200 +JERK_THRESHOLD = 0.2 + + +class LatControlTorque(LatControl): + def __init__(self, CP, CI): + super().__init__(CP, CI) + self.pid = PIDController(CP.lateralTuning.torque.kp, CP.lateralTuning.torque.ki, + k_f=CP.lateralTuning.torque.kf, pos_limit=1.0, neg_limit=-1.0) + self.get_steer_feedforward = CI.get_steer_feedforward_function() + self.steer_max = 1.0 + self.pid.pos_limit = self.steer_max + self.pid.neg_limit = -self.steer_max + self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle + self.friction = CP.lateralTuning.torque.friction + + def reset(self): + super().reset() + self.pid.reset() + + def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): + pid_log = log.ControlsState.LateralTorqueState.new_message() + + if CS.vEgo < MIN_STEER_SPEED or not active: + output_torque = 0.0 + pid_log.active = False + self.pid.reset() + else: + if self.use_steering_angle: + actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) + else: + actual_curvature = llk.angularVelocityCalibrated.value[2] / CS.vEgo + desired_lateral_accel = desired_curvature * CS.vEgo**2 + desired_lateral_jerk = desired_curvature_rate * CS.vEgo**2 + actual_lateral_accel = actual_curvature * CS.vEgo**2 + + setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_curvature + measurement = actual_lateral_accel + LOW_SPEED_FACTOR * actual_curvature + error = setpoint - measurement + pid_log.error = error + + ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY + output_torque = self.pid.update(error, + override=CS.steeringPressed, feedforward=ff, + speed=CS.vEgo, + freeze_integrator=CS.steeringRateLimited) + + friction_compensation = interp(desired_lateral_jerk, [-JERK_THRESHOLD, JERK_THRESHOLD], [-self.friction, self.friction]) + output_torque += friction_compensation + + pid_log.active = True + pid_log.p = self.pid.p + pid_log.i = self.pid.i + pid_log.d = self.pid.d + pid_log.f = self.pid.f + pid_log.output = -output_torque + pid_log.saturated = self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS) + + #TODO left is positive in this convention + return -output_torque, 0.0, pid_log diff --git a/selfdrive/controls/lib/tests/test_latcontrol.py b/selfdrive/controls/lib/tests/test_latcontrol.py index 8345840ec..503eaaa6a 100755 --- a/selfdrive/controls/lib/tests/test_latcontrol.py +++ b/selfdrive/controls/lib/tests/test_latcontrol.py @@ -9,7 +9,7 @@ from selfdrive.car.honda.values import CAR as HONDA from selfdrive.car.toyota.values import CAR as TOYOTA from selfdrive.car.nissan.values import CAR as NISSAN from selfdrive.controls.lib.latcontrol_pid import LatControlPID -from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR +from selfdrive.controls.lib.latcontrol_torque import LatControlTorque from selfdrive.controls.lib.latcontrol_indi import LatControlINDI from selfdrive.controls.lib.latcontrol_angle import LatControlAngle from selfdrive.controls.lib.vehicle_model import VehicleModel @@ -17,7 +17,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel class TestLatControl(unittest.TestCase): - @parameterized.expand([(HONDA.CIVIC, LatControlPID), (TOYOTA.RAV4, LatControlLQR), (TOYOTA.PRIUS, LatControlINDI), (NISSAN.LEAF, LatControlAngle)]) + @parameterized.expand([(HONDA.CIVIC, LatControlPID), (TOYOTA.RAV4, LatControlTorque), (TOYOTA.PRIUS, LatControlINDI), (NISSAN.LEAF, LatControlAngle)]) def test_saturation(self, car_name, controller): CarInterface, CarController, CarState = interfaces[car_name] CP = CarInterface.get_params(car_name) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 098f651a2..de9e404bd 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -22356d49a926a62c01d698d77c1f323016b68fd8 \ No newline at end of file +185f5f9c8d878ad4b98664afc7147400476208cc \ No newline at end of file From a762567de9de52aa6e98818b44a140de6123e6fc Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Tue, 19 Apr 2022 22:45:35 -0700 Subject: [PATCH 11/16] Offline localizer: accept addition camera (#24266) * Offline localizer: add option for additional camera * add names * fix some bugs * Wide ORB features are less accurate * add comment --- selfdrive/locationd/models/constants.py | 9 +++- selfdrive/locationd/models/loc_kf.py | 71 ++++++++++++++++--------- 2 files changed, 55 insertions(+), 25 deletions(-) diff --git a/selfdrive/locationd/models/constants.py b/selfdrive/locationd/models/constants.py index f22f503ee..6d328ce6f 100644 --- a/selfdrive/locationd/models/constants.py +++ b/selfdrive/locationd/models/constants.py @@ -27,9 +27,10 @@ class ObservationKind: PSEUDORANGE_RATE_GLONASS = 21 PSEUDORANGE = 22 PSEUDORANGE_RATE = 23 - ECEF_VEL = 31 + ECEF_VEL = 35 ECEF_ORIENTATION_FROM_GPS = 32 NO_ACCEL = 33 + ORB_FEATURES_WIDE = 34 ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s] ROAD_FRAME_YAW_RATE = 25 # [rad/s] @@ -63,6 +64,8 @@ class ObservationKind: 'imu frame eulers', 'GLONASS pseudorange', 'GLONASS pseudorange rate', + 'pseudorange', + 'pseudorange rate', 'Road Frame x,y speed', 'Road Frame yaw rate', @@ -72,6 +75,10 @@ class ObservationKind: 'Steer Ratio', 'Road Frame x speed', 'Road Roll', + 'ECEF orientation from GPS', + 'NO accel', + 'ORB features wide camera', + 'ECEF_VEL', ] @classmethod diff --git a/selfdrive/locationd/models/loc_kf.py b/selfdrive/locationd/models/loc_kf.py index b6293d60d..8391426dd 100755 --- a/selfdrive/locationd/models/loc_kf.py +++ b/selfdrive/locationd/models/loc_kf.py @@ -50,6 +50,8 @@ class States(): CLOCK_ACCELERATION = slice(28, 29) # clock acceleration in light-meters/s**2, ACCELEROMETER_SCALE_UNUSED = slice(29, 30) # scale of mems accelerometer ACCELEROMETER_BIAS = slice(30, 33) # bias of mems accelerometer + # TODO the offset is likely a translation of the sensor, not a rotation of the camera + WIDE_CAM_OFFSET = slice(33, 36) # wide camera offset angles in radians (tici only) # We curently do not use ACCELEROMETER_SCALE to avoid instability due to too many free variables (ACCELEROMETER_SCALE, ACCELEROMETER_BIAS, IMU_OFFSET). # From experiments we see that ACCELEROMETER_BIAS is more correct than ACCELEROMETER_SCALE @@ -70,6 +72,7 @@ class States(): CLOCK_ACCELERATION_ERR = slice(27, 28) ACCELEROMETER_SCALE_ERR_UNUSED = slice(28, 29) ACCELEROMETER_BIAS_ERR = slice(29, 32) + WIDE_CAM_OFFSET_ERR = slice(32, 35) class LocKalman(): @@ -87,6 +90,7 @@ class LocKalman(): 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0], dtype=np.float64) # state covariance @@ -99,11 +103,12 @@ class LocKalman(): 0.02**2, 2**2, 2**2, 2**2, 0.01**2, - (0.01)**2, (0.01)**2, (0.01)**2, + 0.01**2, 0.01**2, 0.01**2, 10**2, 1**2, 0.2**2, 0.05**2, - 0.05**2, 0.05**2, 0.05**2]) + 0.05**2, 0.05**2, 0.05**2, + 0.01**2, 0.01**2, 0.01**2]) # process noise Q = np.diag([0.03**2, 0.03**2, 0.03**2, @@ -119,10 +124,11 @@ class LocKalman(): (.1)**2, (.01)**2, 0.005**2, (0.02 / 100)**2, - (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2]) + (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2, + (0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2]) # measurements that need to pass mahalanobis distance outlier rejector - maha_test_kinds = [ObservationKind.ORB_FEATURES] # , ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_RATE] + maha_test_kinds = [ObservationKind.ORB_FEATURES, ObservationKind.ORB_FEATURES_WIDE] # , ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_RATE] dim_augment = 7 dim_augment_err = 6 @@ -154,12 +160,14 @@ class LocKalman(): roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :] acceleration = state[States.ACCELERATION, :] imu_angles = state[States.IMU_OFFSET, :] - imu_angles[0, 0] = 0 - imu_angles[2, 0] = 0 + imu_angles[0, 0] = 0 # not observable enough + imu_angles[2, 0] = 0 # not observable enough glonass_bias = state[States.GLONASS_BIAS, :] glonass_freq_slope = state[States.GLONASS_FREQ_SLOPE, :] ca = state[States.CLOCK_ACCELERATION, :] accel_bias = state[States.ACCELEROMETER_BIAS, :] + wide_cam_angles = state[States.WIDE_CAM_OFFSET, :] + wide_cam_angles[0, 0] = 0 # not observable enough dt = sp.Symbol('dt') @@ -308,22 +316,29 @@ class LocKalman(): [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None], [h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]] + wide_cam_rot = euler_rotate(*wide_cam_angles) # MSCKF configuration if N > 0: # experimentally found this is correct value for imx298 with 910 focal length # this is a variable so it can change with focus, but we disregard that for now + # TODO: this isn't correct for tici focal_scale = 1.01 # Add observation functions for orb feature tracks track_epos_sym = sp.MatrixSymbol('track_epos_sym', 3, 1) track_x, track_y, track_z = track_epos_sym h_track_sym = sp.Matrix(np.zeros(((1 + N) * 2, 1))) + h_track_wide_cam_sym = sp.Matrix(np.zeros(((1 + N) * 2, 1))) + track_pos_sym = sp.Matrix([track_x - x, track_y - y, track_z - z]) track_pos_rot_sym = quat_rot.T * track_pos_sym + track_pos_rot_wide_cam_sym = wide_cam_rot * track_pos_rot_sym h_track_sym[-2:, :] = sp.Matrix([focal_scale * (track_pos_rot_sym[1] / track_pos_rot_sym[0]), - focal_scale * (track_pos_rot_sym[2] / track_pos_rot_sym[0])]) + focal_scale * (track_pos_rot_sym[2] / track_pos_rot_sym[0])]) + h_track_wide_cam_sym[-2:, :] = sp.Matrix([focal_scale * (track_pos_rot_wide_cam_sym[1] / track_pos_rot_wide_cam_sym[0]), + focal_scale * (track_pos_rot_wide_cam_sym[2] / track_pos_rot_wide_cam_sym[0])]) h_msckf_test_sym = sp.Matrix(np.zeros(((1 + N) * 3, 1))) - h_msckf_test_sym[-3:, :] = sp.Matrix([track_x - x, track_y - y, track_z - z]) + h_msckf_test_sym[-3:, :] = track_pos_sym for n in range(N): idx = dim_main + n * dim_augment @@ -333,19 +348,23 @@ class LocKalman(): quat_rot = quat_rotate(*q) track_pos_sym = sp.Matrix([track_x - x, track_y - y, track_z - z]) track_pos_rot_sym = quat_rot.T * track_pos_sym + track_pos_rot_wide_cam_sym = wide_cam_rot * track_pos_rot_sym h_track_sym[n * 2:n * 2 + 2, :] = sp.Matrix([focal_scale * (track_pos_rot_sym[1] / track_pos_rot_sym[0]), focal_scale * (track_pos_rot_sym[2] / track_pos_rot_sym[0])]) - h_msckf_test_sym[n * 3:n * 3 + 3, :] = sp.Matrix([track_x - x, track_y - y, track_z - z]) + h_track_wide_cam_sym[n * 2: n * 2 + 2, :] = sp.Matrix([focal_scale * (track_pos_rot_wide_cam_sym[1] / track_pos_rot_wide_cam_sym[0]), + focal_scale * (track_pos_rot_wide_cam_sym[2] / track_pos_rot_wide_cam_sym[0])]) + h_msckf_test_sym[n * 3:n * 3 + 3, :] = track_pos_sym obs_eqs.append([h_msckf_test_sym, ObservationKind.MSCKF_TEST, track_epos_sym]) obs_eqs.append([h_track_sym, ObservationKind.ORB_FEATURES, track_epos_sym]) + obs_eqs.append([h_track_wide_cam_sym, ObservationKind.ORB_FEATURES_WIDE, track_epos_sym]) obs_eqs.append([h_track_sym, ObservationKind.FEATURE_TRACK_TEST, track_epos_sym]) - msckf_params = [dim_main, dim_augment, dim_main_err, dim_augment_err, N, [ObservationKind.MSCKF_TEST, ObservationKind.ORB_FEATURES]] + msckf_params = [dim_main, dim_augment, dim_main_err, dim_augment_err, N, [ObservationKind.MSCKF_TEST, ObservationKind.ORB_FEATURES, ObservationKind.ORB_FEATURES_WIDE]] else: msckf_params = None gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, msckf_params, maha_test_kinds) - def __init__(self, generated_dir, N=4, max_tracks=3000): + def __init__(self, generated_dir, N=4): name = f"{self.name}_{N}" self.obs_noise = {ObservationKind.ODOMETRIC_SPEED: np.atleast_2d(0.2**2), @@ -367,7 +386,6 @@ class LocKalman(): if self.N > 0: x_initial, P_initial, Q = self.pad_augmented(self.x_initial, self.P_initial, self.Q) # lgtm[py/mismatched-multiple-assignment] pylint: disable=unbalanced-tuple-unpacking self.computer = LstSqComputer(generated_dir, N) - self.max_tracks = max_tracks self.quaternion_idxs = [3, ] + [(self.dim_main + i * self.dim_augment + 3)for i in range(self.N)] @@ -427,7 +445,7 @@ class LocKalman(): r = self.predict_and_update_pseudorange(data, t, kind) elif kind == ObservationKind.PSEUDORANGE_RATE_GPS or kind == ObservationKind.PSEUDORANGE_RATE_GLONASS: r = self.predict_and_update_pseudorange_rate(data, t, kind) - elif kind == ObservationKind.ORB_FEATURES: + elif kind == ObservationKind.ORB_FEATURES or kind == ObservationKind.ORB_FEATURES_WIDE: r = self.predict_and_update_orb_features(data, t, kind) elif kind == ObservationKind.MSCKF_TEST: r = self.predict_and_update_msckf_test(data, t, kind) @@ -492,8 +510,12 @@ class LocKalman(): ecef_pos[:] = np.nan poses = self.x[self.dim_main:].reshape((-1, 7)) times = tracks.reshape((len(tracks), self.N + 1, 4))[:, :, 0] - good_counter = 0 - if times.any() and np.allclose(times[0, :-1], self.filter.get_augment_times(), rtol=1e-6): + if kind==ObservationKind.ORB_FEATURES: + pt_std = 0.005 + else: + pt_std = 0.02 + if times.any(): + assert np.allclose(times[0, :-1], self.filter.get_augment_times(), atol=1e-7, rtol=0.0) for i, track in enumerate(tracks): img_positions = track.reshape((self.N + 1, 4))[:, 2:] @@ -502,20 +524,21 @@ class LocKalman(): ecef_pos[i] = self.computer.compute_pos(poses, img_positions[:-1]) z[i] = img_positions.flatten() - R[i, :, :] = np.diag([0.005**2] * (k)) - if np.isfinite(ecef_pos[i][0]): - good_counter += 1 - if good_counter > self.max_tracks: - break + R[i, :, :] = np.diag([pt_std**2] * (k)) + good_idxs = np.all(np.isfinite(ecef_pos), axis=1) + + # This code relies on wide and narrow orb features being captured at the same time, + # and wide features to be processed first. + ret = self.filter.predict_and_update_batch(t, kind, z[good_idxs], R[good_idxs], ecef_pos[good_idxs], + augment=kind==ObservationKind.ORB_FEATURES) + if ret is None: + return + # have to do some weird stuff here to keep # to have the observations input from mesh3d # consistent with the outputs of the filter # Probably should be replaced, not sure how. - ret = self.filter.predict_and_update_batch(t, kind, z[good_idxs], R[good_idxs], ecef_pos[good_idxs], augment=True) - if ret is None: - return - y_full = np.zeros((z.shape[0], z.shape[1] - 3)) if sum(good_idxs) > 0: y_full[good_idxs] = np.array(ret[6]) From 6de434f2ecee89e86c011bece57cb205c4c047e3 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 19 Apr 2022 23:18:01 -0700 Subject: [PATCH 12/16] Honda: do same standstill check as panda (#24262) * check standstill against panda * use XMISSION_SPEED for all, and compare to 0 * fix * only check for Honda for now * update comment * update refs * update refs and update check * phantom commit --- selfdrive/car/honda/carstate.py | 12 ++++-------- selfdrive/car/tests/test_models.py | 2 ++ selfdrive/test/process_replay/ref_commit | 2 +- 3 files changed, 7 insertions(+), 9 deletions(-) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 31a641e90..5314fe375 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -181,18 +181,14 @@ class CarState(CarStateBase): self.prev_cruise_setting = self.cruise_setting # ******************* parse out can ******************* - # TODO: find wheels moving bit in dbc + # STANDSTILL->WHEELS_MOVING bit can be noisy around zero, so use XMISSION_SPEED + # panda checks if the signal is non-zero + ret.standstill = cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] < 1e-5 if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E): - ret.standstill = cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] < 0.1 ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]["DRIVERS_DOOR_OPEN"]) - elif self.CP.carFingerprint == CAR.ODYSSEY_CHN: - ret.standstill = cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] < 0.1 - ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]["DRIVERS_DOOR_OPEN"]) - elif self.CP.carFingerprint in (CAR.FREED, CAR.HRV): - ret.standstill = not cp.vl["STANDSTILL"]["WHEELS_MOVING"] + elif self.CP.carFingerprint in (CAR.ODYSSEY_CHN, CAR.FREED, CAR.HRV): ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]["DRIVERS_DOOR_OPEN"]) else: - ret.standstill = not cp.vl["STANDSTILL"]["WHEELS_MOVING"] ret.doorOpen = any([cp.vl["DOORS_STATUS"]["DOOR_OPEN_FL"], cp.vl["DOORS_STATUS"]["DOOR_OPEN_FR"], cp.vl["DOORS_STATUS"]["DOOR_OPEN_RL"], cp.vl["DOORS_STATUS"]["DOOR_OPEN_RR"]]) ret.seatbeltUnlatched = bool(cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_LAMP"] or not cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_LATCHED"]) diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index b9c36db07..962778097 100755 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -245,6 +245,8 @@ class TestCarModel(unittest.TestCase): if self.CP.carName == "honda": checks['mainOn'] += CS.cruiseState.available != self.safety.get_acc_main_on() + # TODO: fix standstill mismatches for other makes + checks['standstill'] += CS.standstill == self.safety.get_vehicle_moving() CS_prev = CS diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index de9e404bd..bd6c3befc 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -185f5f9c8d878ad4b98664afc7147400476208cc \ No newline at end of file +ea48ff4478914b6aff56a979a647fe13b4993711 \ No newline at end of file From e95a250bcad5f07b7092ba6faf989352d933f678 Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Wed, 20 Apr 2022 05:23:27 -0700 Subject: [PATCH 13/16] Carla pip dependency and fix for git init (#24258) * Add carla pip dependency for dev * Revert other packages in pipfile * Remove redundant carla install in docker * Fix CI Remove broken openpilot_build * New run command --- Dockerfile.openpilot_base | 3 ++- Pipfile | 1 + Pipfile.lock | 15 ++++++++++++++- tools/openpilot_build.sh | 1 - tools/sim/Dockerfile.sim | 4 ---- 5 files changed, 17 insertions(+), 7 deletions(-) delete mode 100644 tools/openpilot_build.sh diff --git a/Dockerfile.openpilot_base b/Dockerfile.openpilot_base index 2ea26b854..c5a024db8 100644 --- a/Dockerfile.openpilot_base +++ b/Dockerfile.openpilot_base @@ -25,8 +25,9 @@ RUN cd /tmp && \ rm -rf /tmp/* && \ rm -rf /root/.cache && \ pip uninstall -y pipenv && \ - # remove unused architectures from gcc for panda cd /usr/lib/gcc/arm-none-eabi/9.2.1 && \ rm -rf arm/ && \ rm -rf thumb/nofp thumb/v6* thumb/v8* thumb/v7+fp thumb/v7-r+fp.sp +RUN sudo git config --global --add safe.directory /tmp/openpilot + diff --git a/Pipfile b/Pipfile index 02e8c602d..6d30179b9 100644 --- a/Pipfile +++ b/Pipfile @@ -37,6 +37,7 @@ breathe = "*" subprocess32 = "*" tenacity = "*" mpld3 = "*" +carla = "==0.9.12" [packages] atomicwrites = "*" diff --git a/Pipfile.lock b/Pipfile.lock index da2dae92a..2c62e13df 100644 --- a/Pipfile.lock +++ b/Pipfile.lock @@ -1,7 +1,7 @@ { "_meta": { "hash": { - "sha256": "7288746fb2afc988e4de2a7d1d3bc2fbef09ce65ca135b6762f3d722c156661b" + "sha256": "41e285b10b9b5a353b3eb9c202886e52d22403c369784877aaa35e099aa203a8" }, "pipfile-spec": 6, "requires": { @@ -1143,6 +1143,19 @@ "index": "pypi", "version": "==4.33.1" }, + "carla": { + "hashes": [ + "sha256:1ed11b56c781cd15cbd540cacfb59ad348e0a021d898cfd0ff89a585f144da0b", + "sha256:20c1e1b72034175824d89b2d86b09ae72b4aca09ea25874dc6251f239297251d", + "sha256:6d1122c24af4f6375dc6858fbb0309b61c219b101d8c5a540def4c36c4563fe1", + "sha256:9c19ebf6cbbc535bde4baf9e18c72ab349657b34c4202b9751541e4c0d20b3cc", + "sha256:a69f6d84b59e2f805b2a417de98f977fe9efe0cfa733da8d75e20d28892da915", + "sha256:c3ae0dce3f1354b6311fee21a365947b0ff169249993a913904f676046d2d69f", + "sha256:dd392a267e14b785a8f65dafef86e05a92201253e9fb4a01e1e262834f20bed2" + ], + "index": "pypi", + "version": "==0.9.12" + }, "certifi": { "hashes": [ "sha256:78884e7c1d4b00ce3cea67b44566851c4343c120abd683433ce934a68ea58872", diff --git a/tools/openpilot_build.sh b/tools/openpilot_build.sh deleted file mode 100644 index f0f89d936..000000000 --- a/tools/openpilot_build.sh +++ /dev/null @@ -1 +0,0 @@ -docker run --rm -v $(pwd):/tmp/openpilot -it commaai/openpilot bash -c 'cd /tmp/openpilot && scons -j$(nproc)' diff --git a/tools/sim/Dockerfile.sim b/tools/sim/Dockerfile.sim index 2e36085b7..921e546d0 100644 --- a/tools/sim/Dockerfile.sim +++ b/tools/sim/Dockerfile.sim @@ -36,10 +36,6 @@ ENV QTWEBENGINE_DISABLE_SANDBOX 1 RUN dbus-uuidgen > /etc/machine-id -# Install CARLA python api -RUN pip install --upgrade pip && \ - pip install --no-cache-dir carla==0.9.12 - # get same tmux config used on NEOS for debugging RUN cd $HOME && \ curl -O https://raw.githubusercontent.com/commaai/eon-neos-builder/master/devices/eon/home/.tmux.conf From 6ab7b2f32562fdb155a14e64c5c2b963f9345b33 Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Wed, 20 Apr 2022 06:03:23 -0700 Subject: [PATCH 14/16] swaglog: delay creating zmq socket to first use - fix modeld crash in sim (#24271) * Initialize zmq socket later with initialize method * empty lines * Add lock and move initialize function * Check for initialized --- selfdrive/common/statlog.cc | 3 +++ selfdrive/common/swaglog.cc | 6 ++---- selfdrive/common/util.h | 22 ++++++++++++++++------ 3 files changed, 21 insertions(+), 10 deletions(-) diff --git a/selfdrive/common/statlog.cc b/selfdrive/common/statlog.cc index 27dfc2ca9..54463e981 100644 --- a/selfdrive/common/statlog.cc +++ b/selfdrive/common/statlog.cc @@ -17,6 +17,9 @@ class StatlogState : public LogState { static StatlogState s = {}; static void log(const char* metric_type, const char* metric, const char* fmt, ...) { + std::lock_guard lk(s.lock); + if (!s.initialized) s.initialize(); + char* value_buf = nullptr; va_list args; va_start(args, fmt); diff --git a/selfdrive/common/swaglog.cc b/selfdrive/common/swaglog.cc index 30e087dc2..21115da10 100644 --- a/selfdrive/common/swaglog.cc +++ b/selfdrive/common/swaglog.cc @@ -21,7 +21,6 @@ class SwaglogState : public LogState { public: SwaglogState() : LogState("ipc:///tmp/logmessage") {} - bool initialized = false; json11::Json::object ctx_j; inline void initialize() { @@ -56,8 +55,7 @@ class SwaglogState : public LogState { } else { ctx_j["device"] = "pc"; } - - initialized = true; + LogState::initialize(); } }; @@ -113,7 +111,7 @@ void cloudlog_t_common(int levelnum, const char* filename, int lineno, const cha char* msg_buf = nullptr; int ret = vasprintf(&msg_buf, fmt, args); if (ret <= 0 || !msg_buf) return; - json11::Json::object tspt_j = json11::Json::object{ + json11::Json::object tspt_j = json11::Json::object{ {"event", msg_buf}, {"time", std::to_string(nanos_since_boot())} }; diff --git a/selfdrive/common/util.h b/selfdrive/common/util.h index bf0df3bca..f3a24723b 100644 --- a/selfdrive/common/util.h +++ b/selfdrive/common/util.h @@ -168,12 +168,18 @@ void update_max_atomic(std::atomic& max, T const& value) { class LogState { public: + bool initialized = false; std::mutex lock; - void *zctx; - void *sock; + void *zctx = nullptr; + void *sock = nullptr; int print_level; + const char* endpoint; - LogState(const char* endpoint) { + LogState(const char* _endpoint) { + endpoint = _endpoint; + } + + inline void initialize() { zctx = zmq_ctx_new(); sock = zmq_socket(zctx, ZMQ_PUSH); @@ -182,9 +188,13 @@ class LogState { zmq_setsockopt(sock, ZMQ_LINGER, &timeout, sizeof(timeout)); zmq_connect(sock, endpoint); - }; + initialized = true; + } + ~LogState() { - zmq_close(sock); - zmq_ctx_destroy(zctx); + if (initialized) { + zmq_close(sock); + zmq_ctx_destroy(zctx); + } } }; From e5c50477efcfc8e7f7481120f9e05f428a6a6179 Mon Sep 17 00:00:00 2001 From: Jason Young Date: Wed, 20 Apr 2022 14:45:59 -0400 Subject: [PATCH 15/16] bump opendbc --- opendbc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc b/opendbc index 9f3902657..abbdc1dae 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 9f3902657df4ff2d359d6c1686d7c008d27b7c7a +Subproject commit abbdc1daee6f6f0d6cbea9cfb2a3777e12dff35d From bfd4fb12eb255f4c9f32cb54a68aa185f3c293e0 Mon Sep 17 00:00:00 2001 From: Jason Young Date: Wed, 20 Apr 2022 15:45:00 -0400 Subject: [PATCH 16/16] support both analog and digital clusters --- selfdrive/car/volkswagen/carcontroller.py | 7 ++++++- selfdrive/car/volkswagen/carstate.py | 6 ++++++ selfdrive/car/volkswagen/volkswagencan.py | 4 ++-- 3 files changed, 14 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 6eda1fc08..badf748eb 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -77,9 +77,14 @@ class CarController(): acc_hold_type, stopping_distance, idx)) if frame % P.ACC_HUD_STEP == 0: + if lead_visible: + lead_distance = 512 if CS.digital_cluster_installed else 8 # TODO: look up actual distance to lead + else: + lead_distance = 0 + idx = (frame / P.ACC_HUD_STEP) % 16 can_sends.append(volkswagencan.create_mqb_acc_02_control(self.packer_pt, CANBUS.pt, CS.tsk_status, - set_speed * CV.MS_TO_KPH, speed_visible, lead_visible, + set_speed * CV.MS_TO_KPH, speed_visible, lead_distance, idx)) can_sends.append(volkswagencan.create_mqb_acc_04_control(self.packer_pt, CANBUS.pt, CS.acc_04_stock_values, idx)) diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 27eace20a..98e35d832 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -16,6 +16,7 @@ class CarState(CarStateBase): self.shifter_values = can_define.dv["EV_Gearshift"]["GearPosition"] self.hca_status_values = can_define.dv["LH_EPS_03"]["EPS_HCA_Status"] self.buttonStates = BUTTON_STATES.copy() + self.digital_cluster_installed = False def update(self, pt_cp, cam_cp, ext_cp, trans_type): ret = car.CarState.new_message() @@ -145,6 +146,9 @@ class CarState(CarStateBase): # Additional safety checks performed in CarInterface. ret.espDisabled = pt_cp.vl["ESP_21"]["ESP_Tastung_passiv"] != 0 + # Update misc car configuration/equipment info + self.digital_cluster_installed = bool(pt_cp.vl["Kombi_03"]["KBI_Variante"]) + return ret @staticmethod @@ -180,6 +184,7 @@ class CarState(CarStateBase): ("ESP_Haltebestaetigung", "ESP_21"), # ESP hold confirmation ("KBI_MFA_v_Einheit_02", "Einheiten_01"), # MPH vs KMH speed display ("KBI_Handbremse", "Kombi_01"), # Manual handbrake applied + ("KBI_Variante", "Kombi_03"), # Digital/full-screen instrument cluster installed ("TSK_Status", "TSK_06"), # ACC engagement status from drivetrain coordinator ("GRA_Hauptschalter", "GRA_ACC_01"), # ACC button, on/off ("GRA_Abbrechen", "GRA_ACC_01"), # ACC button, cancel @@ -210,6 +215,7 @@ class CarState(CarStateBase): ("Kombi_01", 2), # From J285 Instrument cluster ("Blinkmodi_02", 1), # From J519 BCM (sent at 1Hz when no lights active, 50Hz when active) ("Einheiten_01", 1), # From J??? not known if gateway, cluster, or BCM + ("Kombi_03", 1), # From J285 instrument cluster ] if CP.transmissionType == TransmissionType.automatic: diff --git a/selfdrive/car/volkswagen/volkswagencan.py b/selfdrive/car/volkswagen/volkswagencan.py index ea7847488..2f29e258d 100644 --- a/selfdrive/car/volkswagen/volkswagencan.py +++ b/selfdrive/car/volkswagen/volkswagencan.py @@ -48,13 +48,13 @@ def create_mqb_acc_buttons_control(packer, bus, buttonStatesToSend, CS, idx): } return packer.make_can_msg("GRA_ACC_01", bus, values, idx) -def create_mqb_acc_02_control(packer, bus, acc_status, set_speed, speed_visible, lead_visible, idx): +def create_mqb_acc_02_control(packer, bus, acc_status, set_speed, speed_visible, lead_distance, idx): values = { "ACC_Status_Anzeige": 3 if acc_status == 5 else acc_status, "ACC_Wunschgeschw": 327.36 if not speed_visible else set_speed, "ACC_Gesetzte_Zeitluecke": 3, "ACC_Display_Prio": 3, - "ACC_Abstandsindex": 637 if lead_visible else 0, + "ACC_Abstandsindex": lead_distance, } return packer.make_can_msg("ACC_02", bus, values, idx)