diff --git a/.github/ISSUE_TEMPLATE/bug_report.yml b/.github/ISSUE_TEMPLATE/bug_report.yml index 6bbbbacb70..b1a14076ea 100644 --- a/.github/ISSUE_TEMPLATE/bug_report.yml +++ b/.github/ISSUE_TEMPLATE/bug_report.yml @@ -22,23 +22,11 @@ body: validations: required: true - - type: dropdown - id: hw - attributes: - label: What hardware does this issue affect? - multiple: true - options: - - comma three - - comma two - - EON Gold - validations: - required: true - - type: input id: route attributes: label: Provide a route where the issue occurs - description: Ensure the route is fully uploaded at https://useradmin.comma.ai + description: Ensure the route is fully uploaded at https://useradmin.comma.ai. We cannot look into issues without routes, or at least a Dongle ID. placeholder: 77611a1fac303767|2020-05-11--16-37-07 validations: required: true diff --git a/.github/ISSUE_TEMPLATE/car_bug_report.yml b/.github/ISSUE_TEMPLATE/car_bug_report.yml index a48f984192..23527c3a43 100644 --- a/.github/ISSUE_TEMPLATE/car_bug_report.yml +++ b/.github/ISSUE_TEMPLATE/car_bug_report.yml @@ -21,18 +21,6 @@ body: validations: required: true - - type: dropdown - id: hw - attributes: - label: What hardware does this issue affect? - multiple: true - options: - - comma three - - comma two - - EON Gold - validations: - required: true - - type: input id: car attributes: diff --git a/Pipfile b/Pipfile index 473b873077..af3e02f54a 100644 --- a/Pipfile +++ b/Pipfile @@ -82,6 +82,7 @@ tqdm = "*" urllib3 = "*" utm = "*" websocket_client = "*" +hatanaka = "*" [requires] python_version = "3.8" diff --git a/Pipfile.lock b/Pipfile.lock index 4398c37d29..5ee3f2b031 100644 --- a/Pipfile.lock +++ b/Pipfile.lock @@ -1,7 +1,7 @@ { "_meta": { "hash": { - "sha256": "918c8cba5c6a0242dc0f6ea74246176a1c54f0a9395feddf35af2189cc813378" + "sha256": "19a7b58f24cd7542ccb9fd386c7716d77fff3c1f87de496f3f42753cf34a5dde" }, "pipfile-spec": 6, "requires": { @@ -281,6 +281,16 @@ "index": "pypi", "version": "==20.1.0" }, + "hatanaka": { + "hashes": [ + "sha256:0e095d35ed4f607eb77ae47ecb310e4c25f5a6267037b703ea258ed03e5c47da", + "sha256:84faa953b4f641a6d3cf8187f1775ba7e7f8d815f7bcd48cfb18553b766cbc41", + "sha256:ccf8be554deee2fc70be52bd2f1d3d4dd370001caa74333bf041933d69a19023", + "sha256:ce1628029c6b50c142a8fc5f15453c4cf2a3fd88a7128075415aeb5c9a2727d0" + ], + "index": "pypi", + "version": "==2.8.0" + }, "hexdump": { "hashes": [ "sha256:d781a43b0c16ace3f9366aade73e8ad3a7bd5137d58f0b45ab2d3f54876f20db" @@ -304,12 +314,20 @@ "markers": "python_version < '3.10'", "version": "==4.11.3" }, + "importlib-resources": { + "hashes": [ + "sha256:b6062987dfc51f0fcb809187cffbd60f35df7acb4589091f154214af6d0d49d3", + "sha256:e447dc01619b1e951286f3929be820029d48c75eb25d265c28b92a16548212b8" + ], + "markers": "python_version >= '3.7'", + "version": "==5.7.1" + }, "isort": { "hashes": [ "sha256:6f62d78e2f89b4500b080fe3a81690850cd254227f27f75c3a0c491a1f351ba7", "sha256:e8443a5e7a020e9d7f97f1d7d9cd17c88bcb3bc7e218bf9cf5095fe550be2951" ], - "markers": "python_version < '4.0' and python_full_version >= '3.6.1'", + "markers": "python_version < '4' and python_full_version >= '3.6.1'", "version": "==5.10.1" }, "itsdangerous": { @@ -449,6 +467,47 @@ ], "version": "==1.2.1" }, + "ncompress": { + "hashes": [ + "sha256:0349d7de11edd70a7efea9ce9dc67f0e47b5774832dd063f7ae68a9e3e36ea31", + "sha256:070044eab19586a45d1855c3e50e000ce86d6075b122a5ec8cffd480713dffac", + "sha256:13fa26ec8000d786a8079bb265508b5df4b445a4f460481a13549b4bd3c83824", + "sha256:15f10fbfa11345ff0af090e3e6ae13a1fe2b52a2bb39d4f2373c2d6aeac75e5d", + "sha256:2a104803fbe3ab0a96edb14927fa22c8142be838aabe7e938b4a52a4e82db56e", + "sha256:34754041d9bac2d6908ae0d07ba541e4d6d606cca222ddd53f3a57e15f386b0a", + "sha256:34c6496168fd4dbc13f1fc0c0fcbadded1957639956f8cbc6894c39999817e36", + "sha256:3590e66313041721ae81e72ece06b7048c9293321bb30900358638673608e264", + "sha256:393cc3c126b9451fb32fe2bc07773264c90e73afbd37da0df472ac23bfd1a2d5", + "sha256:5336a8831a7e587829ce54e9e27d1fb2e04ddbc7d2d983693e35a3a03ac3ce79", + "sha256:5a2ae8a9170fa1f45df7efa292eb8c437b18c225b63d4adca4f50f9da0e8e0c7", + "sha256:6540556d47670a8fb93878a44d0206bbdc87f32e4c5b57d6fe63691efafbb982", + "sha256:66d991155a1655ccd98e8433c4a7e811d63eb649adb55f47d8f9528a30cc4b7a", + "sha256:736dbae078107742cf6ac7ccc11ae9c5eab77ef2c02aab3ef64802877bb01cab", + "sha256:7608fbda43d04d9f476be2dbf4ef3c96e72d83b9557a48b07fbc9ff3ad29cdd2", + "sha256:78674f246878938387b6f82b10d1aa2192e02544d214541943d12ef1a45e66c6", + "sha256:8322482e72ac2802d1dca1007ec06aa281a4d5cf1cf9f8c75bb51e917382b756", + "sha256:8b9acc46cf36bb998ed215d6e76a94e2bd1e827b9a4cb5362982b7004b5a7620", + "sha256:8eb4a55cbeaeb238a3b412952077be6b3f37b3416cd0211cc22776391ff2fef7", + "sha256:916671d62167191af58d6b4a17b1c09c647e349dcff1fc0b7d764aa64c3773ee", + "sha256:94b3f4e851f5b37e1d4cf2d8da911fa10783a59cba3d7f1f2ae5bd2842558077", + "sha256:9cd040ad73a3b0e917e01cdfba507e10e0bb56849daaac3ac3d86382d4d7ad82", + "sha256:9d89acf209858e7940223cf35324e1b2effec119bb009a41f039e2ea4db22177", + "sha256:9da7c81313aed4b6c6e8020442ed8d03d04bff72947f9380ea1ce2c63ffb8ad1", + "sha256:aaa18a509d9fc173b4b47d53c834e43ced1eda63d2aa7d4613dc59b2f802a31a", + "sha256:ab9fc62baaa55faf8ed8ac67f2c64a7295fec91d7c1f306ac46aa894ca4edf91", + "sha256:af0011bae90e44121f4e4edbff3dccdce7e4c5fc5e354db7eb48410d71f496df", + "sha256:b031e06b42037b181e3514261e1e85a9eae4af990c12b9348a9f22b8042201ff", + "sha256:d11df815d280985dfa660974df11dbe051a1a18dca2f91f9d30fbd6548237b8f", + "sha256:d45ec59a8a3ce00613df0c81e5567854574dbbbf01ecd1a5a0929cd8fb04844d", + "sha256:da216a53db7cd4c0247376f87367dd71df457443567e55310f6d3d23a9aff2f2", + "sha256:e0ebd71990ef7909b6627b5341a2fe1977dcce61dd3760a29e19e3f9e4c6a275", + "sha256:e6f5bf381412e9d3847b76e8b6bd1f84dfadcd3d9c25903c8592facb437909a0", + "sha256:e7bbf10cca1376f4f17ae2c447e33a9d4067525abb0c71d488c9a5ced50552f1", + "sha256:f9ba6ab2aadd6fd90365fdad5219e4dc7bc2459b94f1e900a733dddaf4e9b2e6", + "sha256:fe0a671a2f7dc1ee0438d278ef30ab425a969536100c4352b5cb6bc0b6210818" + ], + "version": "==1.0.0" + }, "nose": { "hashes": [ "sha256:9ff7c6cc443f8c51994b34a667bbcf45afd6d945be7477b52e97516fd17c53ac", diff --git a/cereal b/cereal index 5935572cee..c7d3a0acba 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 5935572cee86f202e2524d5f388f9475f92cd649 +Subproject commit c7d3a0acbae267ef93d30044e1941e060dac9e48 diff --git a/docs/CARS.md b/docs/CARS.md index 14664d480c..8a78afe5cb 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -47,7 +47,7 @@ How We Rate The Cars |Kia|Niro Electric 2019-22|All|||||| |Kia|Telluride 2020|SCC + LKAS|||||| |Lexus|ES 2019-21|All|||||| -|Lexus|ES Hybrid 2019-21|All|||||| +|Lexus|ES Hybrid 2019-22|All|||||| |Lexus|NX 2020|All|||||| |Lexus|RX 2020-22|All|||||| |Lexus|RX Hybrid 2020-21|All|||||| diff --git a/laika_repo b/laika_repo index 226adc655e..48a9cb686a 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit 226adc655e1488474468a97ab4a7705aad7e5837 +Subproject commit 48a9cb686ae2d12cd830f17c166a8fb9f79ab292 diff --git a/opendbc b/opendbc index e19ba095c3..919154efe2 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit e19ba095c3ee288d629284e24ec7e0deaf645f3f +Subproject commit 919154efe2bd07c4dd124d7e6a11a4afc8685f9d diff --git a/scripts/cell.sh b/scripts/cell.sh new file mode 100755 index 0000000000..cae701eccc --- /dev/null +++ b/scripts/cell.sh @@ -0,0 +1,5 @@ +#!/usr/bin/bash + +nmcli connection modify --temporary lte ipv4.route-metric 1 +nmcli connection modify --temporary lte ipv6.route-metric 1 +nmcli con up lte diff --git a/scripts/clwaste b/scripts/clwaste deleted file mode 100755 index 51770f0e37..0000000000 Binary files a/scripts/clwaste and /dev/null differ diff --git a/scripts/restart_modem.sh b/scripts/restart_modem.sh deleted file mode 100755 index fac54b32ff..0000000000 --- a/scripts/restart_modem.sh +++ /dev/null @@ -1,2 +0,0 @@ -#!/usr/bin/bash -echo "restart" > /sys/kernel/debug/msm_subsys/modem diff --git a/scripts/throttling.sh b/scripts/throttling.sh deleted file mode 100755 index d100c5f5ab..0000000000 --- a/scripts/throttling.sh +++ /dev/null @@ -1,16 +0,0 @@ -#!/data/data/com.termux/files/usr/bin/bash -watch -n1 ' - cat /sys/kernel/debug/clk/pwrcl_clk/measure - cat /sys/kernel/debug/clk/perfcl_clk/measure - cat /sys/devices/system/cpu/cpu*/cpufreq/scaling_cur_freq - cat /sys/class/kgsl/kgsl-3d0/gpuclk - echo - echo -n "CPU0 " ; cat /sys/devices/virtual/thermal/thermal_zone5/temp - echo -n "CPU1 " ; cat /sys/devices/virtual/thermal/thermal_zone7/temp - echo -n "CPU2 " ; cat /sys/devices/virtual/thermal/thermal_zone10/temp - echo -n "CPU3 " ; cat /sys/devices/virtual/thermal/thermal_zone12/temp - echo -n "MEM " ; cat /sys/devices/virtual/thermal/thermal_zone2/temp - echo -n "GPU " ; cat /sys/devices/virtual/thermal/thermal_zone16/temp - echo -n "BAT " ; cat /sys/devices/virtual/thermal/thermal_zone29/temp -' - diff --git a/scripts/waste b/scripts/waste deleted file mode 100755 index e3154ab01f..0000000000 Binary files a/scripts/waste and /dev/null differ diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc index 77b0548873..676e740811 100644 --- a/selfdrive/camerad/cameras/camera_common.cc +++ b/selfdrive/camerad/cameras/camera_common.cc @@ -36,10 +36,10 @@ public: hdr_ = ci->hdr; snprintf(args, sizeof(args), "-cl-fast-relaxed-math -cl-denorms-are-zero " - "-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d " + "-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d -DFRAME_OFFSET=%d " "-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DRGB_STRIDE=%d " "-DBAYER_FLIP=%d -DHDR=%d -DCAM_NUM=%d", - ci->frame_width, ci->frame_height, ci->frame_stride, + ci->frame_width, ci->frame_height, ci->frame_stride, ci->frame_offset, b->rgb_width, b->rgb_height, b->rgb_stride, ci->bayer_flip, ci->hdr, s->camera_num); const char *cl_file = "cameras/real_debayer.cl"; @@ -81,7 +81,7 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, frame_buf_count = frame_cnt; // RAW frame - const int frame_size = ci->frame_height * ci->frame_stride; + const int frame_size = (ci->frame_height + ci->extra_height) * ci->frame_stride; camera_bufs = std::make_unique(frame_buf_count); camera_bufs_metadata = std::make_unique(frame_buf_count); diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h index 9c7bf6b034..8c836e0bb8 100644 --- a/selfdrive/camerad/cameras/camera_common.h +++ b/selfdrive/camerad/cameras/camera_common.h @@ -52,11 +52,15 @@ const bool env_log_raw_frames = getenv("LOG_RAW_FRAMES") != NULL; typedef void (*release_cb)(void *cookie, int buf_idx); typedef struct CameraInfo { - int frame_width, frame_height; - int frame_stride; + uint32_t frame_width, frame_height; + uint32_t frame_stride; bool bayer; int bayer_flip; bool hdr; + uint32_t frame_offset = 0; + uint32_t extra_height = 0; + int registers_offset = -1; + int stats_offset = -1; } CameraInfo; typedef struct FrameMetadata { diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc index 7120bf9576..ce9b5663d8 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.cc +++ b/selfdrive/camerad/cameras/camera_qcom2.cc @@ -31,6 +31,9 @@ const size_t FRAME_WIDTH = 1928; const size_t FRAME_HEIGHT = 1208; const size_t FRAME_STRIDE = 2896; // for 12 bit output. 1928 * 12 / 8 + 4 (alignment) +const size_t AR0231_REGISTERS_HEIGHT = 2; +const size_t AR0231_STATS_HEIGHT = 2; + const int MIPI_SETTLE_CNT = 33; // Calculated by camera_freqs.py CameraInfo cameras_supported[CAMERA_ID_MAX] = { @@ -38,17 +41,24 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = { .frame_width = FRAME_WIDTH, .frame_height = FRAME_HEIGHT, .frame_stride = FRAME_STRIDE, + .extra_height = AR0231_REGISTERS_HEIGHT + AR0231_STATS_HEIGHT, + + .registers_offset = 0, + .frame_offset = AR0231_REGISTERS_HEIGHT, + .stats_offset = AR0231_REGISTERS_HEIGHT + FRAME_HEIGHT, + .bayer = true, .bayer_flip = 1, - .hdr = false + .hdr = false, }, [CAMERA_ID_IMX390] = { .frame_width = FRAME_WIDTH, .frame_height = FRAME_HEIGHT, .frame_stride = FRAME_STRIDE, + .bayer = true, .bayer_flip = 1, - .hdr = false + .hdr = false, }, }; @@ -509,10 +519,10 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b if (io_mem_handle != 0) { io_cfg[0].mem_handle[0] = io_mem_handle; io_cfg[0].planes[0] = (struct cam_plane_cfg){ - .width = FRAME_WIDTH, - .height = FRAME_HEIGHT, - .plane_stride = FRAME_STRIDE, - .slice_height = FRAME_HEIGHT, + .width = ci.frame_width, + .height = ci.frame_height + ci.extra_height, + .plane_stride = ci.frame_stride, + .slice_height = ci.frame_height + ci.extra_height, .meta_stride = 0x0, // YUV has meta(stride=0x400, size=0x5000) .meta_size = 0x0, .meta_offset = 0x0, @@ -682,23 +692,23 @@ void CameraState::camera_open() { .lane_cfg = 0x3210, .vc = 0x0, - .dt = 0x2C, // CSI_RAW12 + .dt = 0x12, // Changing stats to 0x2C doesn't work, so change pixels to 0x12 instead .format = CAM_FORMAT_MIPI_RAW_12, .test_pattern = 0x2, // 0x3? .usage_type = 0x0, .left_start = 0, - .left_stop = FRAME_WIDTH - 1, - .left_width = FRAME_WIDTH, + .left_stop = ci.frame_width - 1, + .left_width = ci.frame_width, .right_start = 0, - .right_stop = FRAME_WIDTH - 1, - .right_width = FRAME_WIDTH, + .right_stop = ci.frame_width - 1, + .right_width = ci.frame_width, .line_start = 0, - .line_stop = FRAME_HEIGHT - 1, - .height = FRAME_HEIGHT, + .line_stop = ci.frame_height + ci.extra_height - 1, + .height = ci.frame_height + ci.extra_height, .pixel_clk = 0x0, .batch_size = 0x0, @@ -710,8 +720,8 @@ void CameraState::camera_open() { .data[0] = (struct cam_isp_out_port_info){ .res_type = CAM_ISP_IFE_OUT_RES_RDI_0, .format = CAM_FORMAT_MIPI_RAW_12, - .width = FRAME_WIDTH, - .height = FRAME_HEIGHT, + .width = ci.frame_width, + .height = ci.frame_height + ci.extra_height, .comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0, }, }; @@ -935,6 +945,70 @@ void cameras_close(MultiCameraState *s) { delete s->pm; } +std::map> CameraState::ar0231_build_register_lut(uint8_t *data) { + // This function builds a lookup table from register address, to a pair of indices in the + // buffer where to read this address. The buffer contains padding bytes, + // as well as markers to indicate the type of the next byte. + // + // 0xAA is used to indicate the MSB of the address, 0xA5 for the LSB of the address. + // Every byte of data (MSB and LSB) is preceded by 0x5A. Specifying an address is optional + // for contigous ranges. See page 27-29 of the AR0231 Developer guide for more information. + + int max_i[] = {1828 / 2 * 3, 1500 / 2 * 3}; + auto get_next_idx = [](int cur_idx) { + return (cur_idx % 3 == 1) ? cur_idx + 2 : cur_idx + 1; // Every third byte is padding + }; + + std::map> registers; + for (int register_row = 0; register_row < 2; register_row++) { + uint8_t *registers_raw = data + ci.frame_stride * register_row; + assert(registers_raw[0] == 0x0a); // Start of line + + int value_tag_count = 0; + int first_val_idx = 0; + uint16_t cur_addr = 0; + + for (int i = 1; i <= max_i[register_row]; i = get_next_idx(get_next_idx(i))) { + int val_idx = get_next_idx(i); + + uint8_t tag = registers_raw[i]; + uint16_t val = registers_raw[val_idx]; + + if (tag == 0xAA) { // Register MSB tag + cur_addr = val << 8; + } else if (tag == 0xA5) { // Register LSB tag + cur_addr |= val; + cur_addr -= 2; // Next value tag will increment address again + } else if (tag == 0x5A) { // Value tag + + // First tag + if (value_tag_count % 2 == 0) { + cur_addr += 2; + first_val_idx = val_idx; + } else { + registers[cur_addr] = std::make_pair(first_val_idx + ci.frame_stride * register_row, val_idx + ci.frame_stride * register_row); + } + + value_tag_count++; + } + } + } + return registers; +} + +std::map CameraState::ar0231_parse_registers(uint8_t *data, std::initializer_list addrs) { + if (ar0231_register_lut.empty()) { + ar0231_register_lut = ar0231_build_register_lut(data); + } + + std::map registers; + for (uint16_t addr : addrs) { + auto offset = ar0231_register_lut[addr]; + registers[addr] = ((uint16_t)data[offset.first] << 8) | data[offset.second]; + } + return registers; +} + void CameraState::handle_camera_event(void *evdat) { if (!enabled) return; struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)evdat; @@ -1114,6 +1188,25 @@ void camera_autoexposure(CameraState *s, float grey_frac) { s->set_camera_exposure(grey_frac); } +static float ar0231_parse_temp_sensor(uint16_t calib1, uint16_t calib2, uint16_t data_reg) { + // See AR0231 Developer Guide - page 36 + float slope = (125.0 - 55.0) / ((float)calib1 - (float)calib2); + float t0 = 55.0 - slope * (float)calib2; + return t0 + slope * (float)data_reg; +} + +static void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed){ + uint8_t *data = (uint8_t*)c->buf.cur_camera_buf->addr + c->ci.registers_offset; + auto registers = c->ar0231_parse_registers(data, {0x2000, 0x2002, 0x20b0, 0x20b2, 0x30c6, 0x30c8, 0x30ca, 0x30cc}); + + uint32_t frame_id = ((uint32_t)registers[0x2000] << 16) | registers[0x2002]; + framed.setFrameIdSensor(frame_id); + + float temp_0 = ar0231_parse_temp_sensor(registers[0x30c6], registers[0x30c8], registers[0x20b0]); + float temp_1 = ar0231_parse_temp_sensor(registers[0x30ca], registers[0x30cc], registers[0x20b2]); + framed.setTemperaturesC({temp_0, temp_1}); +} + static void driver_cam_auto_exposure(CameraState *c, SubMaster &sm) { struct ExpRect {int x1, x2, x_skip, y1, y2, y_skip;}; const CameraBuf *b = &c->buf; @@ -1132,10 +1225,12 @@ static void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) if (env_send_driver) { framed.setImage(get_frame_image(&c->buf)); } + if (c->camera_id == CAMERA_ID_AR0231) { + ar0231_process_registers(s, c, framed); + } s->pm->send("driverCameraState", msg); } -// called by processing_thread void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) { const CameraBuf *b = &c->buf; @@ -1152,6 +1247,11 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) { framed.setTransform(b->yuv_transform.v); LOGT(c->buf.cur_frame_data.frame_id, "%s: Transformed", "RoadCamera"); } + + if (c->camera_id == CAMERA_ID_AR0231) { + ar0231_process_registers(s, c, framed); + } + s->pm->send(c == &s->road_cam ? "roadCameraState" : "wideRoadCameraState", msg); const auto [x, y, w, h] = (c == &s->wide_road_cam) ? std::tuple(96, 250, 1734, 524) : std::tuple(96, 160, 1734, 986); @@ -1221,3 +1321,4 @@ void cameras_run(MultiCameraState *s) { cameras_close(s); } + diff --git a/selfdrive/camerad/cameras/camera_qcom2.h b/selfdrive/camerad/cameras/camera_qcom2.h index a7c64c0665..e0553f000e 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.h +++ b/selfdrive/camerad/cameras/camera_qcom2.h @@ -1,6 +1,8 @@ #pragma once #include +#include +#include #include @@ -56,6 +58,8 @@ public: void camera_init(MultiCameraState *multi_cam_state, VisionIpcServer * v, int camera_id, int camera_num, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type, bool enabled); void camera_close(); + std::map ar0231_parse_registers(uint8_t *data, std::initializer_list addrs); + int32_t session_handle; int32_t sensor_dev_handle; int32_t isp_dev_handle; @@ -75,6 +79,7 @@ public: CameraBuf buf; MemoryManager mm; + private: void config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset); void enqueue_req_multi(int start, int n, bool dp); @@ -84,6 +89,10 @@ private: int sensors_init(); void sensors_poke(int request_id); void sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word); + + // Register parsing + std::map> ar0231_register_lut; + std::map> ar0231_build_register_lut(uint8_t *data); }; typedef struct MultiCameraState { diff --git a/selfdrive/camerad/cameras/camera_replay.cc b/selfdrive/camerad/cameras/camera_replay.cc index a9983fe23e..05bdfb9236 100644 --- a/selfdrive/camerad/cameras/camera_replay.cc +++ b/selfdrive/camerad/cameras/camera_replay.cc @@ -30,9 +30,9 @@ void camera_init(VisionIpcServer *v, CameraState *s, int camera_id, unsigned int } CameraInfo ci = { - .frame_width = s->frame->width, - .frame_height = s->frame->height, - .frame_stride = s->frame->width * 3, + .frame_width = (uint32_t)s->frame->width, + .frame_height = (uint32_t)s->frame->height, + .frame_stride = (uint32_t)s->frame->width * 3, }; s->ci = ci; s->camera_num = camera_id; diff --git a/selfdrive/camerad/cameras/real_debayer.cl b/selfdrive/camerad/cameras/real_debayer.cl index 6452e44ebb..6be53c2144 100644 --- a/selfdrive/camerad/cameras/real_debayer.cl +++ b/selfdrive/camerad/cameras/real_debayer.cl @@ -1,11 +1,14 @@ +#ifdef HALF_AS_FLOAT +#define half float +#define half3 float3 +#else #pragma OPENCL EXTENSION cl_khr_fp16 : enable +#endif -const __constant half3 color_correction[3] = { - // post wb CCM - (half3)(1.82717181, -0.31231438, 0.07307673), - (half3)(-0.5743977, 1.36858544, -0.53183455), - (half3)(-0.25277411, -0.05627105, 1.45875782), -}; +// post wb CCM +const __constant half3 color_correction_0 = (half3)(1.82717181, -0.31231438, 0.07307673); +const __constant half3 color_correction_1 = (half3)(-0.5743977, 1.36858544, -0.53183455); +const __constant half3 color_correction_2 = (half3)(-0.25277411, -0.05627105, 1.45875782); // tone mapping params const half cpk = 0.75; @@ -25,28 +28,28 @@ half mf(half x, half cp) { } half3 color_correct(half3 rgb) { - half3 ret = (0,0,0); + half3 ret = (half3)(0.0, 0.0, 0.0); half cpx = 0.01; - ret += (half)rgb.x * color_correction[0]; - ret += (half)rgb.y * color_correction[1]; - ret += (half)rgb.z * color_correction[2]; + ret += (half)rgb.x * color_correction_0; + ret += (half)rgb.y * color_correction_1; + ret += (half)rgb.z * color_correction_2; ret.x = mf(ret.x, cpx); ret.y = mf(ret.y, cpx); ret.z = mf(ret.z, cpx); - ret = clamp(0.0h, 255.0h, ret*255.0h); + ret = clamp(0.0, 255.0, ret*255.0); return ret; } inline half val_from_10(const uchar * source, int gx, int gy, half black_level) { // parse 12bit - int start = gy * FRAME_STRIDE + (3 * (gx / 2)); + int start = gy * FRAME_STRIDE + (3 * (gx / 2)) + (FRAME_STRIDE * FRAME_OFFSET); int offset = gx % 2; uint major = (uint)source[start + offset] << 4; uint minor = (source[start + 2] >> (4 * offset)) & 0xf; half pv = (half)((major + minor)/4); // normalize - pv = max(0.0h, pv - black_level); + pv = max((half)0.0, pv - black_level); pv /= (1024.0f - black_level); // correct vignetting @@ -67,7 +70,7 @@ inline half val_from_10(const uchar * source, int gx, int gy, half black_level) pv = s * pv; } - pv = clamp(0.0h, 1.0h, pv); + pv = clamp((half)0.0, (half)1.0, pv); return pv; } @@ -104,35 +107,33 @@ __kernel void debayer10(const __global uchar * in, half pv = val_from_10(in, x_global, y_global, black_level); cached[localOffset] = pv; - // don't care - if (x_global < 1 || x_global >= RGB_WIDTH - 1 || y_global < 1 || y_global >= RGB_HEIGHT - 1) { - return; - } - // cache padding int localColOffset = -1; int globalColOffset = -1; + const int x_global_mod = (x_global == 0 || x_global == RGB_WIDTH - 1) ? -1: 1; + const int y_global_mod = (y_global == 0 || y_global == RGB_HEIGHT - 1) ? -1: 1; + // cache padding if (x_local < 1) { localColOffset = x_local; globalColOffset = -1; - cached[(y_local + 1) * localRowLen + x_local] = val_from_10(in, x_global-1, y_global, black_level); + cached[(y_local + 1) * localRowLen + x_local] = val_from_10(in, x_global-x_global_mod, y_global, black_level); } else if (x_local >= get_local_size(0) - 1) { localColOffset = x_local + 2; globalColOffset = 1; - cached[localOffset + 1] = val_from_10(in, x_global+1, y_global, black_level); + cached[localOffset + 1] = val_from_10(in, x_global+x_global_mod, y_global, black_level); } if (y_local < 1) { - cached[y_local * localRowLen + x_local + 1] = val_from_10(in, x_global, y_global-1, black_level); + cached[y_local * localRowLen + x_local + 1] = val_from_10(in, x_global, y_global-y_global_mod, black_level); if (localColOffset != -1) { - cached[y_local * localRowLen + localColOffset] = val_from_10(in, x_global+globalColOffset, y_global-1, black_level); + cached[y_local * localRowLen + localColOffset] = val_from_10(in, x_global+(x_global_mod*globalColOffset), y_global-y_global_mod, black_level); } } else if (y_local >= get_local_size(1) - 1) { - cached[(y_local + 2) * localRowLen + x_local + 1] = val_from_10(in, x_global, y_global+1, black_level); + cached[(y_local + 2) * localRowLen + x_local + 1] = val_from_10(in, x_global, y_global+y_global_mod, black_level); if (localColOffset != -1) { - cached[(y_local + 2) * localRowLen + localColOffset] = val_from_10(in, x_global+globalColOffset, y_global+1, black_level); + cached[(y_local + 2) * localRowLen + localColOffset] = val_from_10(in, x_global+(x_global_mod*globalColOffset), y_global+y_global_mod, black_level); } } @@ -197,7 +198,7 @@ __kernel void debayer10(const __global uchar * in, } } - rgb = clamp(0.0h, 1.0h, rgb); + rgb = clamp(0.0, 1.0, rgb); rgb = color_correct(rgb); out[out_idx + 0] = (uchar)(rgb.z); diff --git a/selfdrive/camerad/cameras/sensor2_i2c.h b/selfdrive/camerad/cameras/sensor2_i2c.h index e85223243f..650c17cdfa 100644 --- a/selfdrive/camerad/cameras/sensor2_i2c.h +++ b/selfdrive/camerad/cameras/sensor2_i2c.h @@ -7,7 +7,7 @@ struct i2c_random_wr_payload init_array_imx390[] = { {0x2008, 0xd0}, {0x2009, 0x07}, {0x200a, 0x00}, // MODE_VMAX = time between frames {0x200C, 0xe4}, {0x200D, 0x0c}, // MODE_HMAX - // crop + // crop {0x3410, 0x88}, {0x3411, 0x7}, // CROP_H_SIZE {0x3418, 0xb8}, {0x3419, 0x4}, // CROP_V_SIZE {0x0078, 1}, {0x03c0, 1}, @@ -67,7 +67,7 @@ struct i2c_random_wr_payload init_array_ar0231[] = { {0x30A6, 0x0001}, // Y_ODD_INC_ {0x3402, 0x0788}, // X_OUTPUT_CONTROL {0x3404, 0x04B8}, // Y_OUTPUT_CONTROL - {0x3064, 0x1802}, // SMIA_TEST + {0x3064, 0x1982}, // SMIA_TEST {0x30BA, 0x11F2}, // DIGITAL_CTRL // Enable external trigger and disable GPIO outputs @@ -83,10 +83,10 @@ struct i2c_random_wr_payload init_array_ar0231[] = { // Readout Settings {0x31AE, 0x0204}, // SERIAL_FORMAT, 4-lane MIPI {0x31AC, 0x0C0C}, // DATA_FORMAT_BITS, 12 -> 12 - {0x3342, 0x122C}, // MIPI_F1_PDT_EDT - {0x3346, 0x122C}, // MIPI_F2_PDT_EDT - {0x334A, 0x122C}, // MIPI_F3_PDT_EDT - {0x334E, 0x122C}, // MIPI_F4_PDT_EDT + {0x3342, 0x1212}, // MIPI_F1_PDT_EDT + {0x3346, 0x1212}, // MIPI_F2_PDT_EDT + {0x334A, 0x1212}, // MIPI_F3_PDT_EDT + {0x334E, 0x1212}, // MIPI_F4_PDT_EDT {0x3344, 0x0011}, // MIPI_F1_VDT_VC {0x3348, 0x0111}, // MIPI_F2_VDT_VC {0x334C, 0x0211}, // MIPI_F3_VDT_VC @@ -101,6 +101,10 @@ struct i2c_random_wr_payload init_array_ar0231[] = { {0x3370, 0x03B1}, // DBLC {0x3044, 0x0400}, // DARK_CONTROL + // Enable temperature sensor + {0x30B4, 0x0007}, // TEMPSENS0_CTRL_REG + {0x30B8, 0x0007}, // TEMPSENS1_CTRL_REG + // Enable dead pixel correction using // the 1D line correction scheme {0x31E0, 0x0003}, @@ -114,9 +118,7 @@ struct i2c_random_wr_payload init_array_ar0231[] = { {0x100E, 0x07B1}, // FINE_INTEGRATION_TIME3_MIN {0x1010, 0x0139}, // FINE_INTEGRATION_TIME4_MIN {0x3014, 0x08CB}, // FINE_INTEGRATION_TIME_ - {0x321E, 0x08CB}, // FINE_INTEGRATION_TIME2 - {0x321E, 0x08CB}, // FINE_INTEGRATION_TIME3 - {0x321E, 0x0894}, // FINE_INTEGRATION_TIME4 + {0x321E, 0x0894}, // FINE_INTEGRATION_TIME2 {0x31D0, 0x0000}, // COMPANDING, no good in 10 bit? {0x33DA, 0x0000}, // COMPANDING diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index eda00a2ea1..3d6295d77d 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -49,8 +49,7 @@ class CarController: # *** control msgs *** if CC.cruiseControl.cancel: - # TODO: would be better to start from frame_2b3 - can_sends.append(create_wheel_buttons(self.packer, self.frame, cancel=True)) + can_sends.append(create_wheel_buttons(self.packer, CS.button_counter + 1, cancel=True)) # LKAS_HEARTBIT is forwarded by Panda so no need to send it here. # frame is 100Hz (0.01s period) diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 5f83bbde8b..8ddf1c8684 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -53,6 +53,7 @@ class CarState(CarStateBase): ret.cruiseState.speed = cp.vl["DASHBOARD"]["ACC_SPEED_CONFIG_KPH"] * CV.KPH_TO_MS # CRUISE_STATE is a three bit msg, 0 is off, 1 and 2 are Non-ACC mode, 3 and 4 are ACC mode, find if there are other states too ret.cruiseState.nonAdaptive = cp.vl["DASHBOARD"]["CRUISE_STATE"] in (1, 2) + ret.accFaulted = cp.vl["ACC_2"]["ACC_FAULTED"] != 0 ret.steeringTorque = cp.vl["EPS_STATUS"]["TORQUE_DRIVER"] ret.steeringTorqueEps = cp.vl["EPS_STATUS"]["TORQUE_MOTOR"] @@ -69,6 +70,7 @@ class CarState(CarStateBase): self.lkas_counter = cp_cam.vl["LKAS_COMMAND"]["COUNTER"] self.lkas_car_model = cp_cam.vl["LKAS_HUD"]["CAR_MODEL"] self.lkas_status_ok = cp_cam.vl["LKAS_HEARTBIT"]["LKAS_STATUS_OK"] + self.button_counter = cp.vl["WHEEL_BUTTONS"]["COUNTER"] return ret @@ -93,6 +95,7 @@ class CarState(CarStateBase): ("STEERING_RATE", "STEERING"), ("TURN_SIGNALS", "STEERING_LEVERS"), ("ACC_STATUS_2", "ACC_2"), + ("ACC_FAULTED", "ACC_2"), ("HIGH_BEAM_FLASH", "STEERING_LEVERS"), ("ACC_SPEED_CONFIG_KPH", "DASHBOARD"), ("CRUISE_STATE", "DASHBOARD"), @@ -102,6 +105,7 @@ class CarState(CarStateBase): ("COUNTER", "EPS_STATUS",), ("TRACTION_OFF", "TRACTION_BUTTON"), ("SEATBELT_DRIVER_UNLATCHED", "SEATBELT_STATUS"), + ("COUNTER", "WHEEL_BUTTONS"), ] checks = [ @@ -114,6 +118,7 @@ class CarState(CarStateBase): ("ACC_2", 50), ("GEAR", 50), ("ACCEL_GAS_134", 50), + ("WHEEL_BUTTONS", 50), ("DASHBOARD", 15), ("STEERING_LEVERS", 10), ("SEATBELT_STATUS", 2), diff --git a/selfdrive/car/chrysler/chryslercan.py b/selfdrive/car/chrysler/chryslercan.py index 33f614d75e..896a6b15da 100644 --- a/selfdrive/car/chrysler/chryslercan.py +++ b/selfdrive/car/chrysler/chryslercan.py @@ -52,6 +52,6 @@ def create_wheel_buttons(packer, frame, cancel=False): # WHEEL_BUTTONS (571) Message sent to cancel ACC. values = { "ACC_CANCEL": cancel, - "COUNTER": frame % 10 # FIXME: this counter is wrong + "COUNTER": frame % 0x10 } return packer.make_can_msg("WHEEL_BUTTONS", 0, values) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index ecc6ce7e88..281d01026f 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -82,7 +82,10 @@ class CarState(CarStateBase): ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_EPS"] * self.eps_torque_scale # we could use the override bit from dbc, but it's triggered at too high torque values ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD - ret.steerFaultTemporary = cp.vl["EPS_STATUS"]["LKA_STATE"] not in (1, 5) + # steer rate fault, goes to 21 or 25 for 1 frame, then 9 for ~2 seconds + ret.steerFaultTemporary = cp.vl["EPS_STATUS"]["LKA_STATE"] in (0, 9, 21, 25) + # 17 is a fault from a prolonged high torque delta between cmd and user + ret.steerFaultPermanent = cp.vl["EPS_STATUS"]["LKA_STATE"] == 17 if self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC): ret.cruiseState.available = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0 @@ -120,7 +123,6 @@ class CarState(CarStateBase): ret.stockAeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5) ret.espDisabled = cp.vl["ESP_CONTROL"]["TC_DISABLED"] != 0 - # 2 is standby, 10 is active. TODO: check that everything else is really a faulty state if self.CP.enableBsm: ret.leftBlindspot = (cp.vl["BSM"]["L_ADJACENT"] == 1) or (cp.vl["BSM"]["L_APPROACHING"] == 1) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 433058b3c0..559307ef2d 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -87,7 +87,10 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 13.7 tire_stiffness_factor = 0.7933 ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - set_lat_tune(ret.lateralTuning, LatTunes.PID_C) + if candidate in (CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2): + set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=2.4, FRICTION=0.05) + else: + set_lat_tune(ret.lateralTuning, LatTunes.PID_C) elif candidate in (CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2): stop_and_go = True diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py index 50b03b85ad..7411c0c543 100644 --- a/selfdrive/car/toyota/tunes.py +++ b/selfdrive/car/toyota/tunes.py @@ -56,7 +56,7 @@ def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=.1): tune.torque.useSteeringAngle = True tune.torque.kp = 1.0 / MAX_LAT_ACCEL tune.torque.kf = 1.0 / MAX_LAT_ACCEL - tune.torque.ki = 0.25 / MAX_LAT_ACCEL + tune.torque.ki = 0.1 / MAX_LAT_ACCEL tune.torque.friction = FRICTION elif name == LatTunes.INDI_PRIUS: tune.init('indi') diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 2d9948b9ab..e3cb0b091b 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -151,7 +151,7 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { CAR.LEXUS_CTH: ToyotaCarInfo("Lexus CT Hybrid 2017-18", "LSS", footnotes=[Footnote.DSU]), CAR.LEXUS_ESH: ToyotaCarInfo("Lexus ES Hybrid 2017-18", "LSS", footnotes=[Footnote.DSU]), CAR.LEXUS_ES_TSS2: ToyotaCarInfo("Lexus ES 2019-21"), - CAR.LEXUS_ESH_TSS2: ToyotaCarInfo("Lexus ES Hybrid 2019-21"), + CAR.LEXUS_ESH_TSS2: ToyotaCarInfo("Lexus ES Hybrid 2019-22"), CAR.LEXUS_IS: ToyotaCarInfo("Lexus IS 2017-19"), CAR.LEXUS_NX: ToyotaCarInfo("Lexus NX 2018-19", footnotes=[Footnote.DSU]), CAR.LEXUS_NXH: ToyotaCarInfo("Lexus NX Hybrid 2018-19", footnotes=[Footnote.DSU]), @@ -1337,6 +1337,7 @@ FW_VERSIONS = { b'\x01F15264283100\x00\x00\x00\x00', b'\x01F15264286200\x00\x00\x00\x00', b'\x01F15264286100\x00\x00\x00\x00', + b'\x01F15264283200\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'\x028965B0R01500\x00\x00\x00\x008965B0R02500\x00\x00\x00\x00', @@ -1347,6 +1348,7 @@ FW_VERSIONS = { b'\x01896634A62000\x00\x00\x00\x00', b'\x01896634A08000\x00\x00\x00\x00', b'\x01896634A61000\x00\x00\x00\x00', + b'\x01896634A02001\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F0R01100\x00\x00\x00\x00', @@ -1448,22 +1450,26 @@ FW_VERSIONS = { b'\x028966333T0100\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', b'\x028966333V4000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', b'\x02896633T09000\x00\x00\x00\x00897CF3307001\x00\x00\x00\x00', + b'\x01896633T38000\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ b'F152633423\x00\x00\x00\x00\x00\x00', b'F152633680\x00\x00\x00\x00\x00\x00', b'F152633681\x00\x00\x00\x00\x00\x00', + b'F152633F50\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B33252\x00\x00\x00\x00\x00\x00', b'8965B33590\x00\x00\x00\x00\x00\x00', b'8965B33690\x00\x00\x00\x00\x00\x00', + b'8965B33721\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301100\x00\x00\x00\x00', b'\x018821F3301200\x00\x00\x00\x00', b'\x018821F3301300\x00\x00\x00\x00', b'\x018821F3301400\x00\x00\x00\x00', + b'\x018821F6201300\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ b'\x028646F33030D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', @@ -1472,6 +1478,7 @@ FW_VERSIONS = { b'\x028646F3304100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', b'\x028646F3304200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', b'\x028646F3304300\x00\x00\x00\x008646G2601500\x00\x00\x00\x00', + b'\x028646F3309100\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', ], }, CAR.LEXUS_ESH: { @@ -1758,6 +1765,7 @@ FW_VERSIONS = { CAR.PRIUS_TSS2: { (Ecu.engine, 0x700, None): [ b'\x028966347B1000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x028966347C4000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x028966347C6000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x028966347C8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x038966347C0000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710101\x00\x00\x00\x00', diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 4e9d559798..a373384254 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -42,7 +42,8 @@ class LatControlTorque(LatControl): if CS.vEgo < MIN_STEER_SPEED or not active: output_torque = 0.0 pid_log.active = False - self.pid.reset() + if not active: + self.pid.reset() else: if self.use_steering_angle: actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) diff --git a/selfdrive/debug/compare_fingerprints.py b/selfdrive/debug/compare_fingerprints.py deleted file mode 100755 index 5b7ba58b1e..0000000000 --- a/selfdrive/debug/compare_fingerprints.py +++ /dev/null @@ -1,19 +0,0 @@ -#!/usr/bin/env python3 -# flake8: noqa - -# put 2 fingeprints and print the diffs -f1 = { -168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8 -} - -f2 = { -168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8 -} - -for k in f1: - if k not in f2 or f1[k] != f2[k]: - print(k, "not in f2") - -for k in f2: - if k not in f1 or f2[k] != f1[k]: - print(k, "not in f1") diff --git a/selfdrive/debug/internal/core_voltage_sweep.py b/selfdrive/debug/internal/core_voltage_sweep.py deleted file mode 100755 index c7c609560d..0000000000 --- a/selfdrive/debug/internal/core_voltage_sweep.py +++ /dev/null @@ -1,23 +0,0 @@ -#!/usr/bin/env python3 -import os -import sys -import time - -print("starting at") -os.system("cat /sys/kernel/debug/regulator/pm8994_s11/voltage") -print("volts") - -os.system("echo 99e8000.cpr3-ctrl > /sys/devices/soc/spm-regulator-10/regulator/regulator.56/99e8000.cpr3-ctrl-vdd/driver/unbind") -os.system("echo 1 > /sys/kernel/debug/regulator/pm8994_s11/enable") - -if len(sys.argv) > 1: - i = int(sys.argv[1]) - os.system("echo %d %d > /sys/kernel/debug/regulator/pm8994_s11/voltage" % (i,i)) - os.system("cat /sys/kernel/debug/regulator/pm8994_s11/voltage") -else: - for i in range(900000, 465000, -10000): - print("setting voltage to",i) - os.system("echo %d %d > /sys/kernel/debug/regulator/pm8994_s11/voltage" % (i,i)) - os.system("cat /sys/kernel/debug/regulator/pm8994_s11/voltage") - time.sleep(1) - diff --git a/selfdrive/debug/internal/sensor_test_bootloop.py b/selfdrive/debug/internal/sensor_test_bootloop.py deleted file mode 100755 index 36eb112e44..0000000000 --- a/selfdrive/debug/internal/sensor_test_bootloop.py +++ /dev/null @@ -1,67 +0,0 @@ -#!/usr/bin/python3 -import sys -import os -import stat -import subprocess -import json -from common.text_window import TextWindow -import time - -# Required for sensord not to bus-error on startup -# commaai/cereal#22 -try: - os.mkdir("/dev/shm") -except FileExistsError: - pass -except PermissionError: - print("WARNING: failed to make /dev/shm") - -try: - with open('/tmp/sensor-test-results.json') as infile: - data = json.load(infile) -except Exception: - data = {'sensor-pass': 0, 'sensor-fail': 0} - -STARTUP_SCRIPT = "/data/data/com.termux/files/continue.sh" -try: - with open(STARTUP_SCRIPT, 'w') as startup_script: - startup_script.write("#!/usr/bin/bash\n\n/data/openpilot/selfdrive/debug/internal/sensor_test_bootloop.py\n") - os.chmod(STARTUP_SCRIPT, stat.S_IRWXU) -except Exception: - print("Failed to install new startup script -- aborting") - sys.exit(-1) - -sensord_env = {**os.environ, 'SENSOR_TEST': '1'} -process = subprocess.run("./sensord", cwd="/data/openpilot/selfdrive/sensord", env=sensord_env) # pylint: disable=subprocess-run-check - -if process.returncode == 40: - text = "Current run: SUCCESS\n" - data['sensor-pass'] += 1 -else: - text = "Current run: FAIL\n" - data['sensor-fail'] += 1 - - timestr = str(int(time.time())) - with open('/tmp/dmesg-' + timestr + '.log', 'w') as dmesg_out: - subprocess.call('dmesg', stdout=dmesg_out, shell=False) - with open("/tmp/logcat-" + timestr + '.log', 'w') as logcat_out: - subprocess.call(['logcat', '-d'], stdout=logcat_out, shell=False) - -text += "Sensor pass history: " + str(data['sensor-pass']) + "\n" -text += "Sensor fail history: " + str(data['sensor-fail']) + "\n" - -print(text) - -with open('/tmp/sensor-test-results.json', 'w') as outfile: - json.dump(data, outfile, indent=4) - -with TextWindow(text) as status: - for _ in range(100): - if status.get_status() == 1: - with open(STARTUP_SCRIPT, 'w') as startup_script: - startup_script.write("#!/usr/bin/bash\n\ncd /data/openpilot\nexec ./launch_openpilot.sh\n") - os.chmod(STARTUP_SCRIPT, stat.S_IRWXU) - break - time.sleep(0.1) - -subprocess.Popen("reboot") diff --git a/selfdrive/hardware/tici/hardware.py b/selfdrive/hardware/tici/hardware.py index c626c5f825..1005866d69 100644 --- a/selfdrive/hardware/tici/hardware.py +++ b/selfdrive/hardware/tici/hardware.py @@ -468,6 +468,10 @@ class Tici(HardwareBase): sudo_write("performance", "/sys/class/devfreq/soc:qcom,memlat-cpu0/governor") sudo_write("performance", "/sys/class/devfreq/soc:qcom,memlat-cpu4/governor") + # *** VIDC (encoder) config *** + sudo_write("N", "/sys/kernel/debug/msm_vidc/clock_scaling") + sudo_write("Y", "/sys/kernel/debug/msm_vidc/disable_thermal_mitigation") + def configure_modem(self): sim_id = self.get_sim_info().get('sim_id', '') diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py new file mode 100755 index 0000000000..28150bfea9 --- /dev/null +++ b/selfdrive/locationd/laikad.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 +from typing import List + +from cereal import log, messaging +from laika import AstroDog +from laika.helpers import UbloxGnssId +from laika.raw_gnss import GNSSMeasurement, calc_pos_fix, correct_measurements, process_measurements, read_raw_ublox + + +def correct_and_pos_fix(processed_measurements: List[GNSSMeasurement], dog: AstroDog): + # pos fix needs more than 5 processed_measurements + pos_fix = calc_pos_fix(processed_measurements) + + if len(pos_fix) == 0: + return [], [] + est_pos = pos_fix[0][:3] + corrected = correct_measurements(processed_measurements, est_pos, dog) + return calc_pos_fix(corrected), corrected + + +def process_ublox_msg(ublox_msg, dog, ublox_mono_time: int): + if ublox_msg.which == 'measurementReport': + report = ublox_msg.measurementReport + if len(report.measurements) == 0: + return None + new_meas = read_raw_ublox(report) + processed_measurements = process_measurements(new_meas, dog) + + corrected = correct_and_pos_fix(processed_measurements, dog) + pos_fix, _ = corrected + # todo send corrected messages instead of processed_measurements. Need fix for when having less than 6 measurements + correct_meas_msgs = [create_measurement_msg(m) for m in processed_measurements] + # pos fix can be an empty list if not enough correct measurements are available + if len(pos_fix) > 0: + corrected_pos = pos_fix[0][:3].tolist() + else: + corrected_pos = [0., 0., 0.] + dat = messaging.new_message('gnssMeasurements') + dat.gnssMeasurements = { + "position": corrected_pos, + "ubloxMonoTime": ublox_mono_time, + "correctedMeasurements": correct_meas_msgs + } + return dat + + +def create_measurement_msg(meas: GNSSMeasurement): + c = log.GnssMeasurements.CorrectedMeasurement.new_message() + ublox_gnss_id = meas.ublox_gnss_id + if ublox_gnss_id is None: + # todo never happens will fix in later pr + ublox_gnss_id = UbloxGnssId.GPS + c.constellationId = ublox_gnss_id.value + c.svId = int(meas.prn[1:]) + c.glonassFrequency = meas.glonass_freq if meas.ublox_gnss_id == UbloxGnssId.GLONASS else 0 + c.pseudorange = float(meas.observables['C1C']) # todo should be observables_final when using corrected measurements + c.pseudorangeStd = float(meas.observables_std['C1C']) + c.pseudorangeRate = float(meas.observables['D1C']) # todo should be observables_final when using corrected measurements + c.pseudorangeRateStd = float(meas.observables_std['D1C']) + c.satPos = meas.sat_pos_final.tolist() + c.satVel = meas.sat_vel.tolist() + return c + + +def main(): + dog = AstroDog() + sm = messaging.SubMaster(['ubloxGnss']) + pm = messaging.PubMaster(['gnssMeasurements']) + + while True: + sm.update() + + # Todo if no internet available use latest ephemeris + if sm.updated['ubloxGnss']: + ublox_msg = sm['ubloxGnss'] + msg = process_ublox_msg(ublox_msg, dog, sm.logMonoTime['ubloxGnss']) + if msg is None: + msg = messaging.new_message('gnssMeasurements') + pm.send('gnssMeasurements', msg) + + + +if __name__ == "__main__": + main() diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py new file mode 100755 index 0000000000..877623f0bd --- /dev/null +++ b/selfdrive/locationd/test/test_laikad.py @@ -0,0 +1,23 @@ +#!/usr/bin/env python3 +import unittest +from datetime import datetime + +from laika.helpers import UbloxGnssId + +from laika.gps_time import GPSTime +from laika.raw_gnss import GNSSMeasurement +from selfdrive.locationd.laikad import create_measurement_msg + + +class TestLaikad(unittest.TestCase): + + def test_create_msg_without_errors(self): + gpstime = GPSTime.from_datetime(datetime.now()) + meas = GNSSMeasurement('G01', gpstime.week, gpstime.tow, {'C1C': 0., 'D1C': 0.}, {'C1C': 0., 'D1C': 0.}, ublox_gnss_id=UbloxGnssId.GPS) + msg = create_measurement_msg(meas) + + self.assertEqual(msg.constellationId, 'gps') + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/locationd/test/test_ublox_processing.py b/selfdrive/locationd/test/test_ublox_processing.py new file mode 100755 index 0000000000..94fddb9392 --- /dev/null +++ b/selfdrive/locationd/test/test_ublox_processing.py @@ -0,0 +1,80 @@ +import unittest + +import numpy as np + +from laika import AstroDog +from laika.helpers import UbloxGnssId +from laika.raw_gnss import calc_pos_fix, correct_measurements, process_measurements, read_raw_ublox +from selfdrive.test.openpilotci import get_url +from tools.lib.logreader import LogReader + + +def get_gnss_measurements(log_reader): + gnss_measurements = [] + for msg in log_reader: + if msg.which() == "ubloxGnss": + ublox_msg = msg.ubloxGnss + if ublox_msg.which == 'measurementReport': + report = ublox_msg.measurementReport + if len(report.measurements) > 0: + gnss_measurements.append(read_raw_ublox(report)) + return gnss_measurements + + +class TestUbloxProcessing(unittest.TestCase): + NUM_TEST_PROCESS_MEAS = 10 + + @classmethod + def setUpClass(cls): + lr = LogReader(get_url("4cf7a6ad03080c90|2021-09-29--13-46-36", 0)) + cls.gnss_measurements = get_gnss_measurements(lr) + + def test_read_ublox_raw(self): + count_gps = 0 + count_glonass = 0 + for measurements in self.gnss_measurements: + for m in measurements: + if m.ublox_gnss_id == UbloxGnssId.GPS: + count_gps += 1 + elif m.ublox_gnss_id == UbloxGnssId.GLONASS: + count_glonass += 1 + + self.assertEqual(count_gps, 5036) + self.assertEqual(count_glonass, 3651) + + def test_get_fix(self): + dog = AstroDog() + position_fix_found = 0 + count_processed_measurements = 0 + count_corrected_measurements = 0 + position_fix_found_after_correcting = 0 + + pos_ests = [] + for measurements in self.gnss_measurements[:self.NUM_TEST_PROCESS_MEAS]: + processed_meas = process_measurements(measurements, dog) + count_processed_measurements += len(processed_meas) + pos_fix = calc_pos_fix(processed_meas) + if len(pos_fix) > 0 and all(pos_fix[0] != 0): + position_fix_found += 1 + + corrected_meas = correct_measurements(processed_meas, pos_fix[0][:3], dog) + count_corrected_measurements += len(corrected_meas) + + pos_fix = calc_pos_fix(corrected_meas) + if len(pos_fix) > 0 and all(pos_fix[0] != 0): + pos_ests.append(pos_fix[0]) + position_fix_found_after_correcting += 1 + + mean_fix = np.mean(np.array(pos_ests)[:, :3], axis=0) + np.testing.assert_allclose(mean_fix, [-2452306.662377, -4778343.136806, 3428550.090557], rtol=0, atol=1) + + # Note that can happen that there are less corrected measurements compared to processed when they are invalid. + # However, not for the current segment + self.assertEqual(position_fix_found, self.NUM_TEST_PROCESS_MEAS) + self.assertEqual(position_fix_found_after_correcting, self.NUM_TEST_PROCESS_MEAS) + self.assertEqual(count_processed_measurements, 69) + self.assertEqual(count_corrected_measurements, 69) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index d13026c4b3..74d45f01c9 100644 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 import bz2 +import io import json import os import random @@ -23,6 +24,8 @@ NetworkType = log.DeviceState.NetworkType UPLOAD_ATTR_NAME = 'user.upload' UPLOAD_ATTR_VALUE = b'1' +UPLOAD_QLOG_QCAM_MAX_SIZE = 1e7 # 10 MB + allow_sleep = bool(os.getenv("UPLOADER_SLEEP", "1")) force_wifi = os.getenv("FORCEWIFI") is not None fake_upload = os.getenv("FAKEUPLOAD") is not None @@ -121,11 +124,11 @@ class Uploader(): for name, key, fn in upload_files: if any(f in fn for f in self.immediate_folders): - return (key, fn) + return (name, key, fn) for name, key, fn in upload_files: if name in self.immediate_priority: - return (key, fn) + return (name, key, fn) return None @@ -151,10 +154,11 @@ class Uploader(): self.last_resp = FakeResponse() else: with open(fn, "rb") as f: - data = f.read() - if key.endswith('.bz2') and not fn.endswith('.bz2'): - data = bz2.compress(data) + data = bz2.compress(f.read()) + data = io.BytesIO(data) + else: + data = f self.last_resp = requests.put(url, data=data, headers=headers, timeout=10) except Exception as e: @@ -172,7 +176,7 @@ class Uploader(): return self.last_resp - def upload(self, key, fn, network_type, metered): + def upload(self, name, key, fn, network_type, metered): try: sz = os.path.getsize(fn) except OSError: @@ -182,22 +186,15 @@ class Uploader(): cloudlog.event("upload_start", key=key, fn=fn, sz=sz, network_type=network_type, metered=metered) if sz == 0: - try: - # tag files of 0 size as uploaded - setxattr(fn, UPLOAD_ATTR_NAME, UPLOAD_ATTR_VALUE) - except OSError: - cloudlog.event("uploader_setxattr_failed", exc=self.last_exc, key=key, fn=fn, sz=sz) + # tag files of 0 size as uploaded + success = True + elif name in self.immediate_priority and sz > UPLOAD_QLOG_QCAM_MAX_SIZE: + cloudlog.event("uploader_too_large", key=key, fn=fn, sz=sz) success = True else: start_time = time.monotonic() stat = self.normal_upload(key, fn) if stat is not None and stat.status_code in (200, 201, 401, 403, 412): - try: - # tag file as uploaded - setxattr(fn, UPLOAD_ATTR_NAME, UPLOAD_ATTR_VALUE) - except OSError: - cloudlog.event("uploader_setxattr_failed", exc=self.last_exc, key=key, fn=fn, sz=sz) - self.last_filename = fn self.last_time = time.monotonic() - start_time self.last_speed = (sz / 1e6) / self.last_time @@ -207,6 +204,13 @@ class Uploader(): success = False cloudlog.event("upload_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz, network_type=network_type, metered=metered) + if success: + # tag file as uploaded + try: + setxattr(fn, UPLOAD_ATTR_NAME, UPLOAD_ATTR_VALUE) + except OSError: + cloudlog.event("uploader_setxattr_failed", exc=self.last_exc, key=key, fn=fn, sz=sz) + return success def get_msg(self): @@ -258,13 +262,13 @@ def uploader_fn(exit_event): time.sleep(60 if offroad else 5) continue - key, fn = d + name, key, fn = d # qlogs and bootlogs need to be compressed before uploading - if key.endswith('qlog') or (key.startswith('boot/') and not key.endswith('.bz2')): + if key.endswith(('qlog', 'rlog')) or (key.startswith('boot/') and not key.endswith('.bz2')): key += ".bz2" - success = uploader.upload(key, fn, sm['deviceState'].networkType.raw, sm['deviceState'].networkMetered) + success = uploader.upload(name, key, fn, sm['deviceState'].networkType.raw, sm['deviceState'].networkMetered) if success: backoff = 0.1 elif allow_sleep: diff --git a/selfdrive/loggerd/v4l_encoder.cc b/selfdrive/loggerd/v4l_encoder.cc index b61bf01019..99653758a0 100644 --- a/selfdrive/loggerd/v4l_encoder.cc +++ b/selfdrive/loggerd/v4l_encoder.cc @@ -286,7 +286,7 @@ V4LEncoder::V4LEncoder( struct v4l2_control ctrls[] = { { .id = V4L2_CID_MPEG_VIDEO_H264_PROFILE, .value = V4L2_MPEG_VIDEO_H264_PROFILE_HIGH}, { .id = V4L2_CID_MPEG_VIDEO_H264_LEVEL, .value = V4L2_MPEG_VIDEO_H264_LEVEL_UNKNOWN}, - { .id = V4L2_CID_MPEG_VIDC_VIDEO_NUM_P_FRAMES, .value = 15}, + { .id = V4L2_CID_MPEG_VIDC_VIDEO_NUM_P_FRAMES, .value = 14}, { .id = V4L2_CID_MPEG_VIDC_VIDEO_NUM_B_FRAMES, .value = 0}, { .id = V4L2_CID_MPEG_VIDEO_H264_ENTROPY_MODE, .value = V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CABAC}, { .id = V4L2_CID_MPEG_VIDC_VIDEO_H264_CABAC_MODEL, .value = V4L2_CID_MPEG_VIDC_VIDEO_H264_CABAC_MODEL_0}, diff --git a/selfdrive/test/process_replay/.gitignore b/selfdrive/test/process_replay/.gitignore index a35cd58d41..63c37e64e1 100644 --- a/selfdrive/test/process_replay/.gitignore +++ b/selfdrive/test/process_replay/.gitignore @@ -1 +1,2 @@ fakedata/ +debayer_diff.txt diff --git a/selfdrive/test/process_replay/debayer_replay_ref_commit b/selfdrive/test/process_replay/debayer_replay_ref_commit new file mode 100644 index 0000000000..2f37c33669 --- /dev/null +++ b/selfdrive/test/process_replay/debayer_replay_ref_commit @@ -0,0 +1 @@ +14f411de8085d1cc9e467592c90bcaf95447a467 \ No newline at end of file diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 5bed2e765c..ac8f59b26b 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -70d79fbcc2b9ab0af867a7d6f138b58bcaaa3aa8 \ No newline at end of file +10b766fa845934f0258c52cdf2103d0e1a9496c9 \ No newline at end of file diff --git a/selfdrive/test/process_replay/test_debayer.py b/selfdrive/test/process_replay/test_debayer.py new file mode 100755 index 0000000000..6488184263 --- /dev/null +++ b/selfdrive/test/process_replay/test_debayer.py @@ -0,0 +1,215 @@ +#!/usr/bin/env python3 +import os +import sys +import bz2 +import numpy as np + +import pyopencl as cl # install with `PYOPENCL_CL_PRETEND_VERSION=2.0 pip install pyopencl` + +from selfdrive.hardware import PC, TICI +from common.basedir import BASEDIR +from selfdrive.test.openpilotci import BASE_URL, get_url +from selfdrive.version import get_commit +from tools.lib.logreader import LogReader +from tools.lib.filereader import FileReader + +TEST_ROUTE = "8345e3b82948d454|2022-05-04--13-45-33" +SEGMENT = 0 + +FRAME_WIDTH = 1928 +FRAME_HEIGHT = 1208 +FRAME_STRIDE = 2896 + +UV_WIDTH = FRAME_WIDTH // 2 +UV_HEIGHT = FRAME_HEIGHT // 2 +UV_SIZE = UV_WIDTH * UV_HEIGHT + + +def get_frame_fn(ref_commit, test_route, tici=True): + return f"{test_route}_debayer{'_tici' if tici else ''}_{ref_commit}.bz2" + + +def bzip_frames(frames): + data = bytes() + for y, u, v in frames: + data += y.tobytes() + data += u.tobytes() + data += v.tobytes() + return bz2.compress(data) + + +def unbzip_frames(url): + with FileReader(url) as f: + dat = f.read() + + data = bz2.decompress(dat) + + res = [] + for y_start in range(0, len(data), FRAME_WIDTH * FRAME_HEIGHT + UV_SIZE * 2): + u_start = y_start + FRAME_WIDTH * FRAME_HEIGHT + v_start = u_start + UV_SIZE + + y = np.frombuffer(data[y_start: u_start], dtype=np.uint8).reshape((FRAME_HEIGHT, FRAME_WIDTH)) + u = np.frombuffer(data[u_start: v_start], dtype=np.uint8).reshape((UV_HEIGHT, UV_WIDTH)) + v = np.frombuffer(data[v_start: v_start + UV_SIZE], dtype=np.uint8).reshape((UV_HEIGHT, UV_WIDTH)) + + res.append((y, u, v)) + + return res + + +def init_kernels(frame_offset=0): + ctx = cl.create_some_context(interactive=False) + + with open(os.path.join(BASEDIR, 'selfdrive/camerad/cameras/real_debayer.cl')) as f: + build_args = ' -cl-fast-relaxed-math -cl-denorms-are-zero -cl-single-precision-constant' + \ + f' -DFRAME_STRIDE={FRAME_STRIDE} -DRGB_WIDTH={FRAME_WIDTH} -DRGB_HEIGHT={FRAME_HEIGHT} -DFRAME_OFFSET={frame_offset} -DCAM_NUM=0' + if PC: + build_args += ' -DHALF_AS_FLOAT=1 -cl-std=CL2.0' + debayer_prg = cl.Program(ctx, f.read()).build(options=build_args) + + with open(os.path.join(BASEDIR, 'selfdrive/camerad/transforms/rgb_to_yuv.cl')) as f: + build_args = f' -cl-fast-relaxed-math -cl-denorms-are-zero -DWIDTH={FRAME_WIDTH} -DHEIGHT={FRAME_HEIGHT}' + \ + f' -DUV_WIDTH={UV_WIDTH} -DUV_HEIGHT={UV_HEIGHT} -DRGB_STRIDE={FRAME_WIDTH*3}' + \ + f' -DRGB_SIZE={FRAME_WIDTH*FRAME_HEIGHT}' + rgb_to_yuv_prg = cl.Program(ctx, f.read()).build(options=build_args) + + return ctx, debayer_prg, rgb_to_yuv_prg + + +def debayer_frame(ctx, debayer_prg, rgb_to_yuv_prg, data, rgb=False): + q = cl.CommandQueue(ctx) + + rgb_buff = np.empty(FRAME_WIDTH * FRAME_HEIGHT * 3, dtype=np.uint8) + yuv_buff = np.empty(FRAME_WIDTH * FRAME_HEIGHT + UV_SIZE * 2, dtype=np.uint8) + + cam_g = cl.Buffer(ctx, cl.mem_flags.READ_ONLY | cl.mem_flags.COPY_HOST_PTR, hostbuf=data) + rgb_wg = cl.Buffer(ctx, cl.mem_flags.WRITE_ONLY, FRAME_WIDTH * FRAME_HEIGHT * 3) + yuv_g = cl.Buffer(ctx, cl.mem_flags.WRITE_ONLY, FRAME_WIDTH * FRAME_HEIGHT + UV_SIZE * 2) + + local_worksize = (16, 16) if TICI else (8, 8) + local_mem = cl.LocalMemory(648 if TICI else 400) + ev1 = debayer_prg.debayer10(q, (FRAME_WIDTH, FRAME_HEIGHT), local_worksize, cam_g, rgb_wg, local_mem, np.float32(42)) + cl.enqueue_copy(q, rgb_buff, rgb_wg, wait_for=[ev1]).wait() + cl.enqueue_barrier(q) + + rgb_rg = cl.Buffer(ctx, cl.mem_flags.READ_ONLY | cl.mem_flags.COPY_HOST_PTR, hostbuf=rgb_buff) + cl.enqueue_barrier(q) + + ev3 = rgb_to_yuv_prg.rgb_to_yuv(q, (FRAME_WIDTH // 4, FRAME_HEIGHT // 4), None, rgb_rg, yuv_g) + cl.enqueue_barrier(q) + + cl.enqueue_copy(q, yuv_buff, yuv_g, wait_for=[ev3]).wait() + cl.enqueue_barrier(q) + + y = yuv_buff[:FRAME_WIDTH*FRAME_HEIGHT].reshape((FRAME_HEIGHT, FRAME_WIDTH)) + u = yuv_buff[FRAME_WIDTH*FRAME_HEIGHT:FRAME_WIDTH*FRAME_HEIGHT+UV_SIZE].reshape((UV_HEIGHT, UV_WIDTH)) + v = yuv_buff[FRAME_WIDTH*FRAME_HEIGHT+UV_SIZE:].reshape((UV_HEIGHT, UV_WIDTH)) + + if rgb: + return rgb_buff.reshape((FRAME_HEIGHT, FRAME_WIDTH, 3))[:, :, (2, 1, 0)] + else: + return y, u, v + + +def debayer_replay(lr): + ctx, debayer_prg, rgb_to_yuv_prg = init_kernels() + + frames = [] + for m in lr: + if m.which() == 'roadCameraState': + cs = m.roadCameraState + if cs.image: + data = np.frombuffer(cs.image, dtype=np.uint8) + img = debayer_frame(ctx, debayer_prg, rgb_to_yuv_prg, data) + + frames.append(img) + + return frames + + +if __name__ == "__main__": + update = "--update" in sys.argv + replay_dir = os.path.dirname(os.path.abspath(__file__)) + ref_commit_fn = os.path.join(replay_dir, "debayer_replay_ref_commit") + + # load logs + lr = list(LogReader(get_url(TEST_ROUTE, SEGMENT))) + + # run replay + frames = debayer_replay(lr) + + # get diff + failed = False + diff = '' + yuv_i = ['y', 'u', 'v'] + if not update: + with open(ref_commit_fn) as f: + ref_commit = f.read().strip() + frame_fn = get_frame_fn(ref_commit, TEST_ROUTE, tici=TICI) + + try: + cmp_frames = unbzip_frames(BASE_URL + frame_fn) + + if len(frames) != len(cmp_frames): + failed = True + diff += 'amount of frames not equal\n' + + for i, (frame, cmp_frame) in enumerate(zip(frames, cmp_frames)): + for j in range(3): + fr = frame[j] + cmp_f = cmp_frame[j] + if fr.shape != cmp_f.shape: + failed = True + diff += f'frame shapes not equal for ({i}, {yuv_i[j]})\n' + diff += f'{ref_commit}: {cmp_f.shape}\n' + diff += f'HEAD: {fr.shape}\n' + elif not np.array_equal(fr, cmp_f): + failed = True + if np.allclose(fr, cmp_f, atol=1): + diff += f'frames not equal for ({i}, {yuv_i[j]}), but are all close\n' + else: + diff += f'frames not equal for ({i}, {yuv_i[j]})\n' + + frame_diff = np.abs(np.subtract(fr, cmp_f)) + diff_len = len(np.nonzero(frame_diff)[0]) + if diff_len > 1000: + diff += f'different at a large amount of pixels ({diff_len})\n' + else: + diff += 'different at (frame, yuv, pixel, ref, HEAD):\n' + for k in zip(*np.nonzero(frame_diff)): + diff += f'{i}, {yuv_i[j]}, {k}, {cmp_f[k]}, {fr[k]}\n' + + if failed: + print(diff) + with open("debayer_diff.txt", "w") as f: + f.write(diff) + except Exception as e: + print(str(e)) + failed = True + + # upload new refs + if update or (failed and TICI): + from selfdrive.test.openpilotci import upload_file + + print("Uploading new refs") + + frames_bzip = bzip_frames(frames) + + new_commit = get_commit() + frame_fn = os.path.join(replay_dir, get_frame_fn(new_commit, TEST_ROUTE, tici=TICI)) + with open(frame_fn, "wb") as f2: + f2.write(frames_bzip) + + try: + upload_file(frame_fn, os.path.basename(frame_fn)) + except Exception as e: + print("failed to upload", e) + + if update: + with open(ref_commit_fn, 'w') as f: + f.write(str(new_commit)) + + print("\nNew ref commit: ", new_commit) + + sys.exit(int(failed)) diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 830522ec5e..21696645cd 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -181,6 +181,20 @@ class TestOnroad(unittest.TestCase): cpu_ok = check_cpu_usage(proclogs[0], proclogs[-1]) self.assertTrue(cpu_ok) + def test_camera_processing_time(self): + result = "\n" + result += "------------------------------------------------\n" + result += "-------------- Debayer Timing ------------------\n" + result += "------------------------------------------------\n" + + ts = [getattr(getattr(m, m.which()), "processingTime") for m in self.lr if 'CameraState' in m.which()] + self.assertLess(min(ts), 0.025, f"high execution time: {min(ts)}") + result += f"execution time: min {min(ts):.5f}s\n" + result += f"execution time: max {max(ts):.5f}s\n" + result += f"execution time: mean {np.mean(ts):.5f}s\n" + result += "------------------------------------------------\n" + print(result) + def test_mpc_execution_timings(self): result = "\n" result += "------------------------------------------------\n"