diff --git a/RELEASES.md b/RELEASES.md index 0ef0cd5499..c02a54413c 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,5 +1,10 @@ -Version 0.8.15 (20XX-XX-XX) +Version 0.8.15 (2022-XX-XX) ======================== +* New driving model +* New lateral controller based on physical wheel torque model + * Much smoother control, consistent across the speed range + * Effective feedforward that uses road roll + * Simplified tuning, all car-specific parameters can be derived from data Version 0.8.14 (2022-06-01) ======================== diff --git a/SConstruct b/SConstruct index d9d05f7940..e6141f7385 100644 --- a/SConstruct +++ b/SConstruct @@ -1,5 +1,4 @@ import os -import shutil import subprocess import sys import sysconfig @@ -7,6 +6,8 @@ import platform import numpy as np TICI = os.path.isfile('/TICI') +AGNOS = TICI + Decider('MD5-timestamp') AddOption('--test', @@ -56,7 +57,7 @@ real_arch = arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rst if platform.system() == "Darwin": arch = "Darwin" -if arch == "aarch64" and TICI: +if arch == "aarch64" and AGNOS: arch = "larch64" USE_WEBCAM = os.getenv("USE_WEBCAM") is not None @@ -226,7 +227,7 @@ if GetOption('compile_db'): env.CompilationDatabase('compile_commands.json') # Setup cache dir -cache_dir = '/data/scons_cache' if TICI else '/tmp/scons_cache' +cache_dir = '/data/scons_cache' if AGNOS else '/tmp/scons_cache' CacheDir(cache_dir) Clean(["."], cache_dir) diff --git a/cereal b/cereal index f13d16c78a..5fab88302e 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit f13d16c78a69fdef26b7448520c4398515758861 +Subproject commit 5fab88302ec375ba8c1c3fca3d85afee96113bcf diff --git a/common/SConscript b/common/SConscript index ebc0ec79e9..8aee6f42a7 100644 --- a/common/SConscript +++ b/common/SConscript @@ -19,7 +19,6 @@ _common = fxn('common', common_libs, LIBS="json11") files = [ 'clutil.cc', - 'visionimg.cc', ] _gpucommon = fxn('gpucommon', files) diff --git a/common/api/__init__.py b/common/api/__init__.py index 8b83dfc641..fd2e70910e 100644 --- a/common/api/__init__.py +++ b/common/api/__init__.py @@ -22,13 +22,13 @@ class Api(): def request(self, method, endpoint, timeout=None, access_token=None, **params): return api_get(endpoint, method=method, timeout=timeout, access_token=access_token, **params) - def get_token(self): + def get_token(self, expiry_hours=1): now = datetime.utcnow() payload = { 'identity': self.dongle_id, 'nbf': now, 'iat': now, - 'exp': now + timedelta(hours=1) + 'exp': now + timedelta(hours=expiry_hours) } token = jwt.encode(payload, self.private_key, algorithm='RS256') if isinstance(token, bytes): diff --git a/common/modeldata.h b/common/modeldata.h index 20ebda263c..410a69ea65 100644 --- a/common/modeldata.h +++ b/common/modeldata.h @@ -24,8 +24,6 @@ constexpr auto T_IDXS_FLOAT = build_idxs(10.0); constexpr auto X_IDXS = build_idxs(192.0); constexpr auto X_IDXS_FLOAT = build_idxs(192.0); -const int TICI_CAM_WIDTH = 1928; - const mat3 fcam_intrinsic_matrix = (mat3){{2648.0, 0.0, 1928.0 / 2, 0.0, 2648.0, 1208.0 / 2, 0.0, 0.0, 1.0}}; diff --git a/common/params.cc b/common/params.cc index 2d4f62f735..2338969600 100644 --- a/common/params.cc +++ b/common/params.cc @@ -104,7 +104,6 @@ std::unordered_map keys = { {"DoReboot", CLEAR_ON_MANAGER_START}, {"DoShutdown", CLEAR_ON_MANAGER_START}, {"DoUninstall", CLEAR_ON_MANAGER_START}, - {"EnableWideCamera", CLEAR_ON_MANAGER_START}, {"EndToEndToggle", PERSISTENT}, {"ForcePowerDown", CLEAR_ON_MANAGER_START}, {"GitBranch", PERSISTENT}, @@ -159,6 +158,7 @@ std::unordered_map keys = { {"UpdateFailedCount", CLEAR_ON_MANAGER_START}, {"Version", PERSISTENT}, {"VisionRadarToggle", PERSISTENT}, + {"WideCameraOnly", PERSISTENT}, {"ApiCache_Device", PERSISTENT}, {"ApiCache_DriveStats", PERSISTENT}, {"ApiCache_NavDestinations", PERSISTENT}, diff --git a/common/realtime.py b/common/realtime.py index 8976832141..03051c6f67 100644 --- a/common/realtime.py +++ b/common/realtime.py @@ -8,19 +8,14 @@ from typing import Optional, List, Union from setproctitle import getproctitle # pylint: disable=no-name-in-module from common.clock import sec_since_boot # pylint: disable=no-name-in-module, import-error -from selfdrive.hardware import PC, TICI +from selfdrive.hardware import PC # time step for each process DT_CTRL = 0.01 # controlsd DT_MDL = 0.05 # model DT_TRML = 0.5 # thermald and manager - -# driver monitoring -if TICI: - DT_DMON = 0.05 -else: - DT_DMON = 0.1 +DT_DMON = 0.05 # driver monitoring class Priority: diff --git a/common/swaglog.cc b/common/swaglog.cc index f0d0f0f508..75223854fe 100644 --- a/common/swaglog.cc +++ b/common/swaglog.cc @@ -50,11 +50,7 @@ class SwaglogState : public LogState { ctx_j["dirty"] = !getenv("CLEAN"); // device type - if (Hardware::TICI()) { - ctx_j["device"] = "tici"; - } else { - ctx_j["device"] = "pc"; - } + ctx_j["device"] = Hardware::get_name(); LogState::initialize(); } }; diff --git a/common/tests/test_swaglog.cc b/common/tests/test_swaglog.cc index a95ae45e03..b54d1d6338 100644 --- a/common/tests/test_swaglog.cc +++ b/common/tests/test_swaglog.cc @@ -56,10 +56,7 @@ void recv_log(int thread_cnt, int thread_msg_cnt) { REQUIRE(ctx["version"].string_value() == COMMA_VERSION); - std::string device = "pc"; - if (Hardware::TICI()) { - device = "tici"; - } + std::string device = Hardware::get_name(); REQUIRE(ctx["device"].string_value() == device); int thread_id = atoi(msg["msg"].string_value().c_str()); diff --git a/common/transformations/camera.py b/common/transformations/camera.py index 7573877a3e..5d100d1047 100644 --- a/common/transformations/camera.py +++ b/common/transformations/camera.py @@ -1,7 +1,6 @@ import numpy as np import common.transformations.orientation as orient -from selfdrive.hardware import TICI ## -- hardcoded hardware params -- eon_f_focal_length = 910.0 @@ -45,14 +44,9 @@ tici_fcam_intrinsics_inv = np.linalg.inv(tici_fcam_intrinsics) tici_ecam_intrinsics_inv = np.linalg.inv(tici_ecam_intrinsics) -if not TICI: - FULL_FRAME_SIZE = eon_f_frame_size - FOCAL = eon_f_focal_length - fcam_intrinsics = eon_fcam_intrinsics -else: - FULL_FRAME_SIZE = tici_f_frame_size - FOCAL = tici_f_focal_length - fcam_intrinsics = tici_fcam_intrinsics +FULL_FRAME_SIZE = tici_f_frame_size +FOCAL = tici_f_focal_length +fcam_intrinsics = tici_fcam_intrinsics W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1] diff --git a/common/visionimg.cc b/common/visionimg.cc deleted file mode 100644 index 1009767e0d..0000000000 --- a/common/visionimg.cc +++ /dev/null @@ -1,14 +0,0 @@ -#include "common/visionimg.h" - -#include - -EGLImageTexture::EGLImageTexture(const VisionBuf *buf) { - glGenTextures(1, &frame_tex); - glBindTexture(GL_TEXTURE_2D, frame_tex); - glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, buf->width, buf->height, 0, GL_RGB, GL_UNSIGNED_BYTE, nullptr); - glGenerateMipmap(GL_TEXTURE_2D); -} - -EGLImageTexture::~EGLImageTexture() { - glDeleteTextures(1, &frame_tex); -} diff --git a/common/visionimg.h b/common/visionimg.h deleted file mode 100644 index 0cb41a32b8..0000000000 --- a/common/visionimg.h +++ /dev/null @@ -1,16 +0,0 @@ -#pragma once - -#include "cereal/visionipc/visionbuf.h" - -#ifdef __APPLE__ -#include -#else -#include -#endif - -class EGLImageTexture { - public: - EGLImageTexture(const VisionBuf *buf); - ~EGLImageTexture(); - GLuint frame_tex = 0; -}; diff --git a/docs/c_docs.rst b/docs/c_docs.rst index db7100ab27..1e080a826d 100644 --- a/docs/c_docs.rst +++ b/docs/c_docs.rst @@ -50,10 +50,6 @@ soundd .. autodoxygenindex:: :project: selfdrive_ui_soundd -navd -"""" -.. autodoxygenindex:: - :project: selfdrive_ui_navd replay """""" diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 503ebb5d41..a37e27c4fe 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -8,7 +8,7 @@ source "$BASEDIR/launch_env.sh" DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" -function tici_init { +function agnos_init { # wait longer for weston to come up if [ -f "$BASEDIR/prebuilt" ]; then sleep 3 @@ -77,9 +77,7 @@ function launch { export PYTHONPATH="$PWD:$PWD/pyextra" # hardware specific init - if [ -f /TICI ]; then - tici_init - fi + agnos_init # write tmux scrollback to a file tmux capture-pane -pq -S-1000 > /tmp/launch_log diff --git a/release/files_common b/release/files_common index e254abec66..974ea27711 100644 --- a/release/files_common +++ b/release/files_common @@ -146,15 +146,11 @@ common/modeldata.h common/mat.h common/timing.h -common/visionimg.cc -common/visionimg.h - common/gpio.cc common/gpio.h common/i2c.cc common/i2c.h - selfdrive/controls/__init__.py selfdrive/controls/controlsd.py selfdrive/controls/plannerd.py @@ -293,6 +289,9 @@ selfdrive/ui/qt/widgets/*.h selfdrive/ui/replay/*.cc selfdrive/ui/replay/*.h +selfdrive/ui/qt/maps/*.cc +selfdrive/ui/qt/maps/*.h + selfdrive/camerad/SConscript selfdrive/camerad/main.cc @@ -362,6 +361,8 @@ selfdrive/modeld/runners/run.h selfdrive/monitoring/dmonitoringd.py selfdrive/monitoring/driver_monitor.py +selfdrive/navd/*.py + selfdrive/assets/.gitignore selfdrive/assets/assets.qrc selfdrive/assets/*.png diff --git a/release/files_pc b/release/files_pc index e401badb80..01ecae4327 100644 --- a/release/files_pc +++ b/release/files_pc @@ -2,8 +2,6 @@ selfdrive/modeld/runners/onnx* third_party/mapbox-gl-native-qt/x86_64/*.so -third_party/qt-plugins/x86_64/geoservices/*.so - third_party/libyuv/x64/** third_party/snpe/x86_64/** third_party/snpe/x86_64-linux-clang/** diff --git a/release/files_tici b/release/files_tici index 75abc13abc..ee1ac63c34 100644 --- a/release/files_tici +++ b/release/files_tici @@ -13,11 +13,3 @@ selfdrive/camerad/cameras/real_debayer.cl selfdrive/ui/qt/spinner_larch64 selfdrive/ui/qt/text_larch64 -selfdrive/ui/qt/maps/*.cc -selfdrive/ui/qt/maps/*.h - -selfdrive/ui/navd/*.cc -selfdrive/ui/navd/*.h -selfdrive/ui/navd/navd -selfdrive/ui/navd/.gitignore - diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index c6936d5d40..ba86e02cc6 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -32,7 +32,7 @@ from common.basedir import PERSIST from common.file_helpers import CallbackReader from common.params import Params from common.realtime import sec_since_boot, set_core_affinity -from selfdrive.hardware import HARDWARE, PC, TICI +from selfdrive.hardware import HARDWARE, PC, AGNOS from selfdrive.loggerd.config import ROOT from selfdrive.loggerd.xattr_cache import getxattr, setxattr from selfdrive.statsd import STATS_DIR @@ -413,8 +413,8 @@ def primeActivated(activated): @dispatcher.add_method def setBandwithLimit(upload_speed_kbps, download_speed_kbps): - if not TICI: - return {"success": 0, "error": "only supported on comma three"} + if not AGNOS: + return {"success": 0, "error": "only supported on AGNOS"} try: HARDWARE.set_bandwidth_limit(upload_speed_kbps, download_speed_kbps) diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 2622f7eba1..2745c037b6 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -405,17 +405,12 @@ void send_peripheral_state(PubMaster *pm, Panda *panda) { auto ps = evt.initPeripheralState(); ps.setPandaType(panda->hw_type); - if (Hardware::TICI()) { - double read_time = millis_since_boot(); - ps.setVoltage(std::atoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input").c_str())); - ps.setCurrent(std::atoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input").c_str())); - read_time = millis_since_boot() - read_time; - if (read_time > 50) { - LOGW("reading hwmon took %lfms", read_time); - } - } else { - ps.setVoltage(pandaState.voltage_pkt); - ps.setCurrent(pandaState.current_pkt); + double read_time = millis_since_boot(); + ps.setVoltage(Hardware::get_voltage()); + ps.setCurrent(Hardware::get_current()); + read_time = millis_since_boot() - read_time; + if (read_time > 50) { + LOGW("reading hwmon took %lfms", read_time); } uint16_t fan_speed_rpm = panda->get_fan_speed(); @@ -534,9 +529,7 @@ void peripheral_control_thread(Panda *panda) { int cur_integ_lines = event.getDriverCameraState().getIntegLines(); float cur_gain = event.getDriverCameraState().getGain(); - if (Hardware::TICI()) { - cur_integ_lines = integ_lines_filter.update(cur_integ_lines * cur_gain); - } + cur_integ_lines = integ_lines_filter.update(cur_integ_lines * cur_gain); last_front_frame_t = event.getLogMonoTime(); if (cur_integ_lines <= CUTOFF_IL) { diff --git a/selfdrive/boardd/main.cc b/selfdrive/boardd/main.cc index b151832b72..6f1f83685b 100644 --- a/selfdrive/boardd/main.cc +++ b/selfdrive/boardd/main.cc @@ -12,7 +12,7 @@ int main(int argc, char *argv[]) { int err; err = util::set_realtime_priority(54); assert(err == 0); - err = util::set_core_affinity({Hardware::TICI() ? 4 : 3}); + err = util::set_core_affinity({4}); assert(err == 0); } diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc index 28e0c3c78f..21e225d538 100644 --- a/selfdrive/camerad/cameras/camera_common.cc +++ b/selfdrive/camerad/cameras/camera_common.cc @@ -16,6 +16,7 @@ #include "common/swaglog.h" #include "common/util.h" #include "selfdrive/hardware/hw.h" +#include "msm_media_info.h" #ifdef QCOM2 #include "CL/cl_ext_qcom.h" @@ -28,17 +29,17 @@ ExitHandler do_exit; class Debayer { public: - Debayer(cl_device_id device_id, cl_context context, const CameraBuf *b, const CameraState *s) { + Debayer(cl_device_id device_id, cl_context context, const CameraBuf *b, const CameraState *s, int buf_width, int uv_offset) { char args[4096]; const CameraInfo *ci = &s->ci; 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_OFFSET=%d " - "-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DRGB_STRIDE=%d " + "-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DRGB_STRIDE=%d -DYUV_STRIDE=%d -DUV_OFFSET=%d " "-DBAYER_FLIP=%d -DHDR=%d -DCAM_NUM=%d%s", ci->frame_width, ci->frame_height, ci->frame_stride, ci->frame_offset, - b->rgb_width, b->rgb_height, b->rgb_stride, + b->rgb_width, b->rgb_height, b->rgb_stride, buf_width, uv_offset, ci->bayer_flip, ci->hdr, s->camera_num, s->camera_num==1 ? " -DVIGNETTING" : ""); const char *cl_file = "cameras/real_debayer.cl"; cl_program prg_debayer = cl_program_from_file(context, device_id, cl_file, args); @@ -89,23 +90,23 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, rgb_width = ci->frame_width; rgb_height = ci->frame_height; - if (!Hardware::TICI() && ci->bayer) { - // debayering does a 2x downscale - rgb_width = ci->frame_width / 2; - rgb_height = ci->frame_height / 2; - } - yuv_transform = get_model_yuv_transform(ci->bayer); vipc_server->create_buffers(rgb_type, UI_BUF_COUNT, true, rgb_width, rgb_height); rgb_stride = vipc_server->get_buffer(rgb_type)->stride; LOGD("created %d UI vipc buffers with size %dx%d", UI_BUF_COUNT, rgb_width, rgb_height); - vipc_server->create_buffers(yuv_type, YUV_BUFFER_COUNT, false, rgb_width, rgb_height); - LOGD("created %d YUV vipc buffers with size %dx%d", YUV_BUFFER_COUNT, rgb_width, rgb_height); + int nv12_width = VENUS_Y_STRIDE(COLOR_FMT_NV12, rgb_width); + int nv12_height = VENUS_Y_SCANLINES(COLOR_FMT_NV12, rgb_height); + assert(nv12_width == VENUS_UV_STRIDE(COLOR_FMT_NV12, rgb_width)); + assert(nv12_height/2 == VENUS_UV_SCANLINES(COLOR_FMT_NV12, rgb_height)); + size_t nv12_size = 2346 * nv12_width; // comes from v4l2_format.fmt.pix_mp.plane_fmt[0].sizeimage + size_t nv12_uv_offset = nv12_width * nv12_height; + vipc_server->create_buffers_with_sizes(yuv_type, YUV_BUFFER_COUNT, false, rgb_width, rgb_height, nv12_size, nv12_width, nv12_uv_offset); + LOGD("created %d YUV vipc buffers with size %dx%d", YUV_BUFFER_COUNT, nv12_width, nv12_height); if (ci->bayer) { - debayer = new Debayer(device_id, context, this, s); + debayer = new Debayer(device_id, context, this, s, nv12_width, nv12_uv_offset); } rgb2yuv = std::make_unique(context, device_id, rgb_width, rgb_height, rgb_stride); @@ -233,20 +234,28 @@ kj::Array get_raw_frame_image(const CameraBuf *b) { } static kj::Array yuv420_to_jpeg(const CameraBuf *b, int thumbnail_width, int thumbnail_height) { + int downscale = b->cur_yuv_buf->width / thumbnail_width; + assert(downscale * thumbnail_height == b->cur_yuv_buf->height); + int in_stride = b->cur_yuv_buf->stride; + // make the buffer big enough. jpeg_write_raw_data requires 16-pixels aligned height to be used. std::unique_ptr buf(new uint8_t[(thumbnail_width * ((thumbnail_height + 15) & ~15) * 3) / 2]); uint8_t *y_plane = buf.get(); uint8_t *u_plane = y_plane + thumbnail_width * thumbnail_height; uint8_t *v_plane = u_plane + (thumbnail_width * thumbnail_height) / 4; { - int result = libyuv::I420Scale( - b->cur_yuv_buf->y, b->rgb_width, b->cur_yuv_buf->u, b->rgb_width / 2, b->cur_yuv_buf->v, b->rgb_width / 2, - b->rgb_width, b->rgb_height, - y_plane, thumbnail_width, u_plane, thumbnail_width / 2, v_plane, thumbnail_width / 2, - thumbnail_width, thumbnail_height, libyuv::kFilterNone); - if (result != 0) { - LOGE("Generate YUV thumbnail failed."); - return {}; + // subsampled conversion from nv12 to yuv + for (int hy = 0; hy < thumbnail_height/2; hy++) { + for (int hx = 0; hx < thumbnail_width/2; hx++) { + int ix = hx * downscale + (downscale-1)/2; + int iy = hy * downscale + (downscale-1)/2; + y_plane[(hy*2 + 0)*thumbnail_width + (hx*2 + 0)] = b->cur_yuv_buf->y[(iy*2 + 0) * in_stride + ix*2 + 0]; + y_plane[(hy*2 + 0)*thumbnail_width + (hx*2 + 1)] = b->cur_yuv_buf->y[(iy*2 + 0) * in_stride + ix*2 + 1]; + y_plane[(hy*2 + 1)*thumbnail_width + (hx*2 + 0)] = b->cur_yuv_buf->y[(iy*2 + 1) * in_stride + ix*2 + 0]; + y_plane[(hy*2 + 1)*thumbnail_width + (hx*2 + 1)] = b->cur_yuv_buf->y[(iy*2 + 1) * in_stride + ix*2 + 1]; + u_plane[hy*thumbnail_width/2 + hx] = b->cur_yuv_buf->uv[iy*in_stride + ix*2 + 0]; + v_plane[hy*thumbnail_width/2 + hx] = b->cur_yuv_buf->uv[iy*in_stride + ix*2 + 1]; + } } } diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h index 85c55f4b03..74d6a6eb3f 100644 --- a/selfdrive/camerad/cameras/camera_common.h +++ b/selfdrive/camerad/cameras/camera_common.h @@ -13,7 +13,6 @@ #include "common/mat.h" #include "common/queue.h" #include "common/swaglog.h" -#include "common/visionimg.h" #include "selfdrive/hardware/hw.h" #define CAMERA_ID_IMX298 0 diff --git a/selfdrive/camerad/cameras/real_debayer.cl b/selfdrive/camerad/cameras/real_debayer.cl index dc6044ed53..fc9f410494 100644 --- a/selfdrive/camerad/cameras/real_debayer.cl +++ b/selfdrive/camerad/cameras/real_debayer.cl @@ -1,7 +1,5 @@ #define UV_WIDTH RGB_WIDTH / 2 #define UV_HEIGHT RGB_HEIGHT / 2 -#define U_OFFSET RGB_WIDTH * RGB_HEIGHT -#define V_OFFSET RGB_WIDTH * RGB_HEIGHT + UV_WIDTH * UV_HEIGHT #define RGB_TO_Y(r, g, b) ((((mul24(b, 13) + mul24(g, 65) + mul24(r, 33)) + 64) >> 7) + 16) #define RGB_TO_U(r, g, b) ((mul24(b, 56) - mul24(g, 37) - mul24(r, 19) + 0x8080) >> 8) @@ -141,17 +139,20 @@ __kernel void debayer10(const __global uchar * in, __global uchar * out) RGB_TO_Y(rgb_out[0].s0, rgb_out[0].s1, rgb_out[0].s2), RGB_TO_Y(rgb_out[1].s0, rgb_out[1].s1, rgb_out[1].s2) ); - vstore2(yy, 0, out + mad24(gid_y * 2, RGB_WIDTH, gid_x * 2)); + vstore2(yy, 0, out + mad24(gid_y * 2, YUV_STRIDE, gid_x * 2)); yy = (uchar2)( RGB_TO_Y(rgb_out[2].s0, rgb_out[2].s1, rgb_out[2].s2), RGB_TO_Y(rgb_out[3].s0, rgb_out[3].s1, rgb_out[3].s2) ); - vstore2(yy, 0, out + mad24(gid_y * 2 + 1, RGB_WIDTH, gid_x * 2)); + vstore2(yy, 0, out + mad24(gid_y * 2 + 1, YUV_STRIDE, gid_x * 2)); // write uvs const short ar = AVERAGE(rgb_out[0].s0, rgb_out[1].s0, rgb_out[2].s0, rgb_out[3].s0); const short ag = AVERAGE(rgb_out[0].s1, rgb_out[1].s1, rgb_out[2].s1, rgb_out[3].s1); const short ab = AVERAGE(rgb_out[0].s2, rgb_out[1].s2, rgb_out[2].s2, rgb_out[3].s2); - out[U_OFFSET + mad24(gid_y, UV_WIDTH, gid_x)] = RGB_TO_U(ar, ag, ab); - out[V_OFFSET + mad24(gid_y, UV_WIDTH, gid_x)] = RGB_TO_V(ar, ag, ab); + uchar2 uv = (uchar2)( + RGB_TO_U(ar, ag, ab), + RGB_TO_V(ar, ag, ab) + ); + vstore2(uv, 0, out + UV_OFFSET + mad24(gid_y, YUV_STRIDE, gid_x * 2)); } diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc index f59e03f8fa..c86543265c 100644 --- a/selfdrive/camerad/main.cc +++ b/selfdrive/camerad/main.cc @@ -7,7 +7,7 @@ #include "selfdrive/hardware/hw.h" int main(int argc, char *argv[]) { - if (Hardware::TICI()) { + if (!Hardware::PC()) { int ret; ret = util::set_realtime_priority(53); assert(ret == 0); diff --git a/selfdrive/camerad/snapshot/snapshot.py b/selfdrive/camerad/snapshot/snapshot.py index db1342ee7d..9f9072c962 100755 --- a/selfdrive/camerad/snapshot/snapshot.py +++ b/selfdrive/camerad/snapshot/snapshot.py @@ -4,13 +4,12 @@ import time import numpy as np from PIL import Image -from typing import List import cereal.messaging as messaging from cereal.visionipc import VisionIpcClient, VisionStreamType from common.params import Params from common.realtime import DT_MDL -from selfdrive.hardware import TICI, PC +from selfdrive.hardware import PC from selfdrive.controls.lib.alertmanager import set_offroad_alert from selfdrive.manager.process_config import managed_processes @@ -44,19 +43,15 @@ def yuv_to_rgb(y, u, v): return rgb.astype(np.uint8) -def extract_image(buf, w, h): - y = np.array(buf[:w*h], dtype=np.uint8).reshape((h, w)) - u = np.array(buf[w*h: w*h+(w//2)*(h//2)], dtype=np.uint8).reshape((h//2, w//2)) - v = np.array(buf[w*h+(w//2)*(h//2):], dtype=np.uint8).reshape((h//2, w//2)) +def extract_image(buf, w, h, stride, uv_offset): + y = np.array(buf[:uv_offset], dtype=np.uint8).reshape((-1, stride))[:h, :w] + u = np.array(buf[uv_offset::2], dtype=np.uint8).reshape((-1, stride//2))[:h//2, :w//2] + v = np.array(buf[uv_offset+1::2], dtype=np.uint8).reshape((-1, stride//2))[:h//2, :w//2] return yuv_to_rgb(y, u, v) -def rois_in_focus(lapres: List[float]) -> float: - return sum(1. / len(lapres) for sharpness in lapres if sharpness >= LM_THRESH) - - -def get_snapshots(frame="roadCameraState", front_frame="driverCameraState", focus_perc_threshold=0.): +def get_snapshots(frame="roadCameraState", front_frame="driverCameraState"): sockets = [s for s in (frame, front_frame) if s is not None] sm = messaging.SubMaster(sockets) vipc_clients = {s: VisionIpcClient("camerad", VISION_STREAMS[s], True) for s in sockets} @@ -68,21 +63,14 @@ def get_snapshots(frame="roadCameraState", front_frame="driverCameraState", focu for client in vipc_clients.values(): client.connect(True) - # wait for focus - start_t = time.monotonic() - while time.monotonic() - start_t < 10: - sm.update(100) - if min(sm.rcv_frame.values()) > 1 and rois_in_focus(sm[frame].sharpnessScore) >= focus_perc_threshold: - break - # grab images rear, front = None, None if frame is not None: c = vipc_clients[frame] - rear = extract_image(c.recv(), c.width, c.height) + rear = extract_image(c.recv(), c.width, c.height, c.stride, c.uv_offset) if front_frame is not None: c = vipc_clients[front_frame] - front = extract_image(c.recv(), c.width, c.height) + front = extract_image(c.recv(), c.width, c.height, c.stride, c.uv_offset) return rear, front @@ -113,11 +101,9 @@ def snapshot(): if not PC: managed_processes['camerad'].start() - frame = "wideRoadCameraState" if TICI else "roadCameraState" + frame = "wideRoadCameraState" front_frame = "driverCameraState" if front_camera_allowed else None - focus_perc_threshold = 0. if TICI else 10 / 12. - - rear, front = get_snapshots(frame, front_frame, focus_perc_threshold) + rear, front = get_snapshots(frame, front_frame) finally: managed_processes['camerad'].stop() params.put_bool("IsTakingSnapshot", False) diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index 7bb91c40f7..806a060f97 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -1,11 +1,40 @@ # functions common among cars +import capnp + from cereal import car from common.numpy_fast import clip -from typing import Dict +from typing import Dict, List # kg of standard extra cargo to count for drive, gas, etc... STD_CARGO_KG = 136. +ButtonType = car.CarState.ButtonEvent.Type +EventName = car.CarEvent.EventName + + +def create_button_event(cur_but: int, prev_but: int, buttons_dict: Dict[int, capnp.lib.capnp._EnumModule], + unpressed: int = 0) -> capnp.lib.capnp._DynamicStructBuilder: + if cur_but != unpressed: + be = car.CarState.ButtonEvent(pressed=True) + but = cur_but + else: + be = car.CarState.ButtonEvent(pressed=False) + but = prev_but + be.type = buttons_dict.get(but, ButtonType.unknown) + return be + + +def create_button_enable_events(buttonEvents: capnp.lib.capnp._DynamicListBuilder) -> List[int]: + events = [] + for b in buttonEvents: + # do enable on both accel and decel buttons + if b.type in (ButtonType.accelCruise, ButtonType.decelCruise) and not b.pressed: + events.append(EventName.buttonEnable) + # do disable on button down + if b.type == ButtonType.cancel and b.pressed: + events.append(EventName.buttonCancel) + return events + def gen_empty_fingerprint(): return {i: {} for i in range(0, 4)} diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 46991bb4d2..85fcfa1fad 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -122,10 +122,13 @@ def fingerprint(logcan, sendcan): finger = gen_empty_fingerprint() candidate_cars = {i: all_legacy_fingerprint_cars() for i in [0, 1]} # attempt fingerprint on both bus 0 and 1 frame = 0 - frame_fingerprint = 10 # 0.1s + frame_fingerprint = 25 # 0.25s car_fingerprint = None done = False + # drain CAN socket so we always get the latest messages + messaging.drain_sock_raw(logcan) + while not done: a = get_one_can(logcan) diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index d00708c336..b4265fc0bd 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -3,12 +3,15 @@ from cereal import car from math import fabs from common.conversions import Conversions as CV -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car import STD_CARGO_KG, create_button_enable_events, create_button_event, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config from selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams from selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName +BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise, + CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel} + class CarInterface(CarInterfaceBase): @staticmethod @@ -159,29 +162,14 @@ class CarInterface(CarInterfaceBase): ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False - buttonEvents = [] - if self.CS.cruise_buttons != self.CS.prev_cruise_buttons and self.CS.prev_cruise_buttons != CruiseButtons.INIT: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.unknown - if self.CS.cruise_buttons != CruiseButtons.UNPRESS: - be.pressed = True - but = self.CS.cruise_buttons - else: - be.pressed = False - but = self.CS.prev_cruise_buttons - if but == CruiseButtons.RES_ACCEL: - if not (ret.cruiseState.enabled and ret.standstill): - be.type = ButtonType.accelCruise # Suppress resume button if we're resuming from stop so we don't adjust speed. - elif but == CruiseButtons.DECEL_SET: - be.type = ButtonType.decelCruise - elif but == CruiseButtons.CANCEL: - be.type = ButtonType.cancel - elif but == CruiseButtons.MAIN: - be.type = ButtonType.altButton3 - buttonEvents.append(be) - - ret.buttonEvents = buttonEvents + be = create_button_event(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT, CruiseButtons.UNPRESS) + + # Suppress resume button if we're resuming from stop so we don't adjust speed. + if be.type == ButtonType.accelCruise and (ret.cruiseState.enabled and ret.standstill): + be.type = ButtonType.unknown + + ret.buttonEvents = [be] events = self.create_common_events(ret, pcm_enable=False) @@ -193,13 +181,7 @@ class CarInterface(CarInterfaceBase): events.add(car.CarEvent.EventName.belowSteerSpeed) # handle button presses - for b in ret.buttonEvents: - # do enable on both accel and decel buttons - if b.type in (ButtonType.accelCruise, ButtonType.decelCruise) and not b.pressed: - events.add(EventName.buttonEnable) - # do disable on button down - if b.type == ButtonType.cancel and b.pressed: - events.add(EventName.buttonCancel) + events.events.extend(create_button_enable_events(ret.buttonEvents)) ret.events = events.to_msg() diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 265175d940..5b97b3389f 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -4,7 +4,7 @@ from panda import Panda from common.conversions import Conversions as CV from common.numpy_fast import interp from selfdrive.car.honda.values import CarControllerParams, CruiseButtons, HondaFlags, CAR, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL -from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car import STD_CARGO_KG, CivicParams, create_button_enable_events, create_button_event, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.disable_ecu import disable_ecu @@ -12,6 +12,8 @@ from selfdrive.car.disable_ecu import disable_ecu ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName TransmissionType = car.CarParams.TransmissionType +BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise, + CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel} class CarInterface(CarInterfaceBase): @@ -334,37 +336,11 @@ class CarInterface(CarInterfaceBase): buttonEvents = [] if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.unknown - if self.CS.cruise_buttons != 0: - be.pressed = True - but = self.CS.cruise_buttons - else: - be.pressed = False - but = self.CS.prev_cruise_buttons - if but == CruiseButtons.RES_ACCEL: - be.type = ButtonType.accelCruise - elif but == CruiseButtons.DECEL_SET: - be.type = ButtonType.decelCruise - elif but == CruiseButtons.CANCEL: - be.type = ButtonType.cancel - elif but == CruiseButtons.MAIN: - be.type = ButtonType.altButton3 - buttonEvents.append(be) + buttonEvents.append(create_button_event(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT)) if self.CS.cruise_setting != self.CS.prev_cruise_setting: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.unknown - if self.CS.cruise_setting != 0: - be.pressed = True - but = self.CS.cruise_setting - else: - be.pressed = False - but = self.CS.prev_cruise_setting - if but == 1: - be.type = ButtonType.altButton1 - # TODO: more buttons? - buttonEvents.append(be) + buttonEvents.append(create_button_event(self.CS.cruise_setting, self.CS.prev_cruise_setting, {1: ButtonType.altButton1})) + ret.buttonEvents = buttonEvents # events @@ -391,16 +367,7 @@ class CarInterface(CarInterfaceBase): events.add(EventName.manualRestart) # handle button presses - for b in ret.buttonEvents: - - # do enable on both accel and decel buttons - if not self.CP.pcmCruise: - if b.type in (ButtonType.accelCruise, ButtonType.decelCruise) and not b.pressed: - events.add(EventName.buttonEnable) - - # do disable on button down - if b.type == ButtonType.cancel and b.pressed: - events.add(EventName.buttonCancel) + events.events.extend(create_button_enable_events(ret.buttonEvents)) ret.events = events.to_msg() diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 170624737d..fb3579464a 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -114,11 +114,10 @@ class CarController: accel = clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) - lead_visible = False stopping = actuators.longControlState == LongCtrlState.stopping set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH) - can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2), lead_visible, - set_speed_in_units, stopping, CS.out.gasPressed)) + can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2), + hud_control.leadVisible, set_speed_in_units, stopping, CS.out.gasPressed)) self.accel = accel # 20 Hz LFA MFA message diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index d03eb135fc..53499053e0 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -86,8 +86,8 @@ def create_acc_commands(packer, enabled, accel, jerk, idx, lead_visible, set_spe "TauGapSet": 4, "VSetDis": set_speed if enabled else 0, "AliveCounterACC": idx % 0x10, - "ObjValid": 1 if lead_visible else 0, - "ACC_ObjStatus": 1 if lead_visible else 0, + "ObjValid": 0, # TODO: these two bits may allow for better longitudinal control + "ACC_ObjStatus": 0, "ACC_ObjLatPos": 0, "ACC_ObjRelSpd": 0, "ACC_ObjDist": 0, diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index b4dc26c87a..673b304e5a 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -4,7 +4,7 @@ from panda import Panda from common.conversions import Conversions as CV from selfdrive.car.hyundai.values import CAR, DBC, HDA2_CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, Buttons, CarControllerParams from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car import STD_CARGO_KG, create_button_enable_events, create_button_event, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.disable_ecu import disable_ecu from selfdrive.controls.lib.latcontrol_torque import set_torque_tune @@ -12,6 +12,8 @@ from selfdrive.controls.lib.latcontrol_torque import set_torque_tune ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName ENABLE_BUTTONS = (Buttons.RES_ACCEL, Buttons.SET_DECEL, Buttons.CANCEL) +BUTTONS_DICT = {Buttons.RES_ACCEL: ButtonType.accelCruise, Buttons.SET_DECEL: ButtonType.decelCruise, + Buttons.GAP_DIST: ButtonType.gapAdjustCruise, Buttons.CANCEL: ButtonType.cancel} class CarInterface(CarInterfaceBase): @@ -338,37 +340,14 @@ class CarInterface(CarInterfaceBase): if self.CS.brake_error: events.add(EventName.brakeUnavailable) - if self.CS.CP.openpilotLongitudinalControl: - buttonEvents = [] - - if self.CS.cruise_buttons[-1] != self.CS.prev_cruise_buttons: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.unknown - if self.CS.cruise_buttons[-1] != 0: - be.pressed = True - but = self.CS.cruise_buttons[-1] - else: - be.pressed = False - but = self.CS.prev_cruise_buttons - if but == Buttons.RES_ACCEL: - be.type = ButtonType.accelCruise - elif but == Buttons.SET_DECEL: - be.type = ButtonType.decelCruise - elif but == Buttons.GAP_DIST: - be.type = ButtonType.gapAdjustCruise - elif but == Buttons.CANCEL: - be.type = ButtonType.cancel - buttonEvents.append(be) - - ret.buttonEvents = buttonEvents - - for b in ret.buttonEvents: - # do enable on both accel and decel buttons - if b.type in (ButtonType.accelCruise, ButtonType.decelCruise) and not b.pressed: - events.add(EventName.buttonEnable) - # do disable on button down - if b.type == ButtonType.cancel and b.pressed: - events.add(EventName.buttonCancel) + if self.CS.CP.openpilotLongitudinalControl and self.CS.cruise_buttons[-1] != self.CS.prev_cruise_buttons: + buttonEvents = [create_button_event(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT)] + # Handle CF_Clu_CruiseSwState changing buttons mid-press + if self.CS.cruise_buttons[-1] != 0 and self.CS.prev_cruise_buttons != 0: + buttonEvents.append(create_button_event(0, self.CS.prev_cruise_buttons, BUTTONS_DICT)) + + ret.buttonEvents = buttonEvents + events.events.extend(create_button_enable_events(ret.buttonEvents)) # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s) if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.: diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 396bfaa924..1cd2acabeb 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -377,7 +377,7 @@ FW_VERSIONS = { b'\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100', b'\xf1\x00DN ESC \x07 104\x19\x08\x01 58910-L0100', b'\xf1\x00DN ESC \x08 103\x19\x06\x01 58910-L1300', - b'\xf1\x8758910-L0100\xf1\x00DN ESC \a 106 \a\x01 58910-L0100', + b'\xf1\x8758910-L0100\xf1\x00DN ESC \x07 106 \x07\x01 58910-L0100', b'\xf1\x8758910-L0100\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100', b'\xf1\x8758910-L0100\xf1\x00DN ESC \x06 106 \x07\x01 58910-L0100', b'\xf1\x8758910-L0100\xf1\x00DN ESC \x07 104\x19\x08\x01 58910-L0100', @@ -398,6 +398,7 @@ FW_VERSIONS = { b'HM6M1_0a0_F00', b'HM6M1_0a0_G20', b'HM6M2_0a0_BD0', + b'\xf1\x8739110-2S278\xf1\x82DNDVD5GMCCXXXL5B', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00DN8 MDPS C 1,00 1,01 56310L0010\x00 4DNAC101', # modified firmware @@ -479,6 +480,7 @@ FW_VERSIONS = { b'\xf1\x87SAMFBA7978674GJ2gw\x87xgw\x97ywwwwvUGeUUeU\x87O\xfb\xff\x98w\x8f\xfffF\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', b'\xf1\x87SAMFBA9283024GJ2wwwwEUuWwwgwwwwwwwww\x87/\xfb\xff\x98w\x8f\xff<\xd3\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', b'\xf1\x87SAMFBA9708354GJ2wwwwVf\x86h\x88wx\x87xww\x87\x88\x88\x88\x88w/\xfa\xff\x97w\x8f\xff\x86\xa0\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', + b'\xf1\x87SANDB45316691GC6\x99\x99\x99\x99\x88\x88\xa8\x8avfwfwwww\x87wxwT\x9f\xfd\xff\x88wo\xff\x1c\xfa\xf1\x89HT6WAD10A1\xf1\x82SDN8G25NB3\x00\x00\x00\x00\x00\x00', ], }, CAR.SONATA_LF: { @@ -946,6 +948,7 @@ FW_VERSIONS = { b'\xf1\x8799110Q4000\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4000 ', b'\xf1\x8799110Q4100\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4100 ', b'\xf1\x8799110Q4500\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4500 ', + b'\xf1\x8799110Q4600\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4600 ', b'\xf1\x8799110Q4600\xf1\x00DEev SCC FNCUP 1.00 1.00 99110-Q4600 ', b'\xf1\x8799110Q4600\xf1\x00DEev SCC FHCUP 1.00 1.00 99110-Q4600 ', ], diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 3d59102e43..1b577cc6b7 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -61,7 +61,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 18.27 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - set_lat_tune(ret.lateralTuning, LatTunes.PID_A) + set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=2.8, FRICTION=0.024) elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RXH, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2): stop_and_go = True diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index d42db26b91..0b6948cf44 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -675,6 +675,7 @@ FW_VERSIONS = { b'\x01896630ZP1000\x00\x00\x00\x00', b'\x01896630ZP2000\x00\x00\x00\x00', b'\x01896630ZQ5000\x00\x00\x00\x00', + b'\x01896630ZU9000\x00\x00\x00\x00', b'\x018966312L8000\x00\x00\x00\x00', b'\x018966312M0000\x00\x00\x00\x00', b'\x018966312M9000\x00\x00\x00\x00', @@ -912,6 +913,7 @@ FW_VERSIONS = { b'\x01F15260E051\x00\x00\x00\x00\x00\x00', b'\x01F15260E061\x00\x00\x00\x00\x00\x00', b'\x01F15260E110\x00\x00\x00\x00\x00\x00', + b'\x01F15260E170\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x700, None): [ b'\x01896630E62100\x00\x00\x00\x00', @@ -931,10 +933,12 @@ FW_VERSIONS = { (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301400\x00\x00\x00\x00', b'\x018821F6201200\x00\x00\x00\x00', + b'\x018821F6201300\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ b'\x028646F0E02100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', b'\x028646F4803000\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', + b'\x028646F4803000\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', ], }, CAR.HIGHLANDERH_TSS2: { @@ -963,10 +967,12 @@ FW_VERSIONS = { (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301400\x00\x00\x00\x00', b'\x018821F6201200\x00\x00\x00\x00', + b'\x018821F6201300\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ b'\x028646F0E02100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', b'\x028646F4803000\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', + b'\x028646F4803000\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', ], }, CAR.LEXUS_IS: { @@ -1372,6 +1378,7 @@ FW_VERSIONS = { b'\x01896634A08000\x00\x00\x00\x00', b'\x01896634A61000\x00\x00\x00\x00', b'\x01896634A02001\x00\x00\x00\x00', + b'\x01896634A03000\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F0R01100\x00\x00\x00\x00', diff --git a/selfdrive/car/vin.py b/selfdrive/car/vin.py index 648f416511..2c5b623b06 100755 --- a/selfdrive/car/vin.py +++ b/selfdrive/car/vin.py @@ -1,25 +1,37 @@ #!/usr/bin/env python3 +import struct import traceback import cereal.messaging as messaging +import panda.python.uds as uds from panda.python.uds import FUNCTIONAL_ADDRS from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery from selfdrive.swaglog import cloudlog -VIN_REQUEST = b'\x09\x02' -VIN_RESPONSE = b'\x49\x02\x01' +OBD_VIN_REQUEST = b'\x09\x02' +OBD_VIN_RESPONSE = b'\x49\x02\x01' + +UDS_VIN_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + struct.pack("!H", uds.DATA_IDENTIFIER_TYPE.VIN) +UDS_VIN_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + struct.pack("!H", uds.DATA_IDENTIFIER_TYPE.VIN) + VIN_UNKNOWN = "0" * 17 def get_vin(logcan, sendcan, bus, timeout=0.1, retry=5, debug=False): - for i in range(retry): - try: - query = IsoTpParallelQuery(sendcan, logcan, bus, FUNCTIONAL_ADDRS, [VIN_REQUEST], [VIN_RESPONSE], functional_addr=True, debug=debug) - for addr, vin in query.get_data(timeout).items(): - return addr[0], vin.decode() - print(f"vin query retry ({i+1}) ...") - except Exception: - cloudlog.warning(f"VIN query exception: {traceback.format_exc()}") + for request, response in ((UDS_VIN_REQUEST, UDS_VIN_RESPONSE), (OBD_VIN_REQUEST, OBD_VIN_RESPONSE)): + for i in range(retry): + try: + query = IsoTpParallelQuery(sendcan, logcan, bus, FUNCTIONAL_ADDRS, [request, ], [response, ], functional_addr=True, debug=debug) + for addr, vin in query.get_data(timeout).items(): + + # Honda Bosch response starts with a length, trim to correct length + if vin.startswith(b'\x11'): + vin = vin[1:18] + + return addr[0], vin.decode() + print(f"vin query retry ({i+1}) ...") + except Exception: + cloudlog.warning(f"VIN query exception: {traceback.format_exc()}") return 0, VIN_UNKNOWN diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index fc0f289e39..edb1538c31 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -27,7 +27,7 @@ from selfdrive.controls.lib.events import Events, ET from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.locationd.calibrationd import Calibration -from selfdrive.hardware import HARDWARE, TICI +from selfdrive.hardware import HARDWARE from selfdrive.manager.process_config import managed_processes SOFT_DISABLE_TIME = 3 # seconds @@ -68,17 +68,14 @@ class Controls: self.pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', 'carControl', 'carEvents', 'carParams']) - self.camera_packets = ["roadCameraState", "driverCameraState"] - if TICI: - self.camera_packets.append("wideRoadCameraState") + self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"] self.can_sock = can_sock if can_sock is None: can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 20 self.can_sock = messaging.sub_sock('can', timeout=can_timeout) - if TICI: - self.log_sock = messaging.sub_sock('androidLog') + self.log_sock = messaging.sub_sock('androidLog') if CI is None: # wait for one pandaState and one CAN packet @@ -358,17 +355,16 @@ class Controls: if planner_fcw or model_fcw: self.events.add(EventName.fcw) - if TICI: - for m in messaging.drain_sock(self.log_sock, wait_for_one=False): - try: - msg = m.androidLog.message - if any(err in msg for err in ("ERROR_CRC", "ERROR_ECC", "ERROR_STREAM_UNDERFLOW", "APPLY FAILED")): - csid = msg.split("CSID:")[-1].split(" ")[0] - evt = CSID_MAP.get(csid, None) - if evt is not None: - self.events.add(evt) - except UnicodeDecodeError: - pass + for m in messaging.drain_sock(self.log_sock, wait_for_one=False): + try: + msg = m.androidLog.message + if any(err in msg for err in ("ERROR_CRC", "ERROR_ECC", "ERROR_STREAM_UNDERFLOW", "APPLY FAILED")): + csid = msg.split("CSID:")[-1].split(" ")[0] + evt = CSID_MAP.get(csid, None) + if evt is not None: + self.events.add(evt) + except UnicodeDecodeError: + pass # TODO: fix simulator if not SIMULATION: diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 8a37f11fdb..7ca05cc744 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -284,7 +284,7 @@ def comm_issue_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaste def camera_malfunction_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: all_cams = ('roadCameraState', 'driverCameraState', 'wideRoadCameraState') - bad_cams = [s for s in all_cams if s in sm.data.keys() and not sm.all_checks([s, ])] + bad_cams = [s.replace('State', '') for s in all_cams if s in sm.data.keys() and not sm.all_checks([s, ])] return NormalPermanentAlert("Camera Malfunction", ', '.join(bad_cams)) diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index aedf61a073..9bbad59084 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -3,20 +3,14 @@ from cereal import log from common.filter_simple import FirstOrderFilter from common.numpy_fast import interp from common.realtime import DT_MDL -from selfdrive.hardware import TICI from selfdrive.swaglog import cloudlog TRAJECTORY_SIZE = 33 # camera offset is meters from center car to camera -# model path is in the frame of the camera. Empirically -# the model knows the difference between TICI and EON -# so a path offset is not needed +# model path is in the frame of the camera PATH_OFFSET = 0.00 -if TICI: - CAMERA_OFFSET = 0.04 -else: - CAMERA_OFFSET = 0.0 +CAMERA_OFFSET = 0.04 class LanePlanner: diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 02f1c19a77..1fc79decef 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -5,12 +5,11 @@ from common.realtime import Priority, config_realtime_process from selfdrive.swaglog import cloudlog from selfdrive.controls.lib.longitudinal_planner import Planner from selfdrive.controls.lib.lateral_planner import LateralPlanner -from selfdrive.hardware import TICI import cereal.messaging as messaging def plannerd_thread(sm=None, pm=None): - config_realtime_process(5 if TICI else 2, Priority.CTRL_LOW) + config_realtime_process(5, Priority.CTRL_LOW) cloudlog.info("plannerd is waiting for CarParams") params = Params() @@ -18,7 +17,7 @@ def plannerd_thread(sm=None, pm=None): cloudlog.info("plannerd got CarParams: %s", CP.carName) use_lanelines = not params.get_bool('EndToEndToggle') - wide_camera = params.get_bool('EnableWideCamera') if TICI else False + wide_camera = params.get_bool('WideCameraOnly') cloudlog.event("e2e mode", on=use_lanelines) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index cae9474faf..0e514193dc 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -11,7 +11,6 @@ from common.realtime import Ratekeeper, Priority, config_realtime_process from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid from selfdrive.controls.lib.radar_helpers import Cluster, Track, RADAR_TO_CAMERA from selfdrive.swaglog import cloudlog -from selfdrive.hardware import TICI class KalmanParams(): @@ -180,7 +179,7 @@ class RadarD(): # fuses camera and radar data for best lead detection def radard_thread(sm=None, pm=None, can_sock=None): - config_realtime_process(5 if TICI else 2, Priority.CTRL_LOW) + config_realtime_process(5, Priority.CTRL_LOW) # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") diff --git a/selfdrive/hardware/__init__.py b/selfdrive/hardware/__init__.py index 03dfce86d7..a1bf2e912f 100644 --- a/selfdrive/hardware/__init__.py +++ b/selfdrive/hardware/__init__.py @@ -6,6 +6,7 @@ from selfdrive.hardware.tici.hardware import Tici from selfdrive.hardware.pc.hardware import Pc TICI = os.path.isfile('/TICI') +AGNOS = TICI PC = not TICI diff --git a/selfdrive/hardware/base.h b/selfdrive/hardware/base.h index 2826d09700..b70948d482 100644 --- a/selfdrive/hardware/base.h +++ b/selfdrive/hardware/base.h @@ -2,6 +2,7 @@ #include #include +#include "cereal/messaging/messaging.h" // no-op base hw class class HardwareNone { @@ -10,6 +11,10 @@ public: static constexpr float MIN_VOLUME = 0.2; static std::string get_os_version() { return ""; } + static std::string get_name() { return ""; }; + static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::UNKNOWN; }; + static int get_voltage() { return 0; }; + static int get_current() { return 0; }; static void reboot() {} static void poweroff() {} @@ -21,4 +26,5 @@ public: static bool PC() { return false; } static bool TICI() { return false; } + static bool AGNOS() { return false; } }; diff --git a/selfdrive/hardware/hw.h b/selfdrive/hardware/hw.h index 5f178ec0f9..364a1c5c33 100644 --- a/selfdrive/hardware/hw.h +++ b/selfdrive/hardware/hw.h @@ -10,8 +10,11 @@ class HardwarePC : public HardwareNone { public: static std::string get_os_version() { return "openpilot for PC"; } + static std::string get_name() { return "pc"; }; + static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::PC; }; static bool PC() { return true; } static bool TICI() { return util::getenv("TICI", 0) == 1; } + static bool AGNOS() { return util::getenv("TICI", 0) == 1; } }; #define Hardware HardwarePC #endif diff --git a/selfdrive/hardware/tici/hardware.h b/selfdrive/hardware/tici/hardware.h index ec53e9ae8f..b11a0a5bb0 100644 --- a/selfdrive/hardware/tici/hardware.h +++ b/selfdrive/hardware/tici/hardware.h @@ -12,9 +12,15 @@ public: static constexpr float MAX_VOLUME = 0.9; static constexpr float MIN_VOLUME = 0.2; static bool TICI() { return true; } + static bool AGNOS() { return true; } static std::string get_os_version() { return "AGNOS " + util::read_file("/VERSION"); }; + static std::string get_name() { return "tici"; }; + static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::TICI; }; + static int get_voltage() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input").c_str()); }; + static int get_current() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input").c_str()); }; + static void reboot() { std::system("sudo reboot"); }; static void poweroff() { std::system("sudo poweroff"); }; diff --git a/selfdrive/hardware/tici/test_power_draw.py b/selfdrive/hardware/tici/test_power_draw.py index d6af25a4e5..934d72e3a1 100755 --- a/selfdrive/hardware/tici/test_power_draw.py +++ b/selfdrive/hardware/tici/test_power_draw.py @@ -22,7 +22,7 @@ PROCS = [ Proc('camerad', 2.25), Proc('modeld', 0.95), Proc('dmonitoringmodeld', 0.25), - Proc('encoderd', 0.42), + Proc('encoderd', 0.23), ] diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 6a8707a6cd..d7bf36fb26 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -20,7 +20,6 @@ from common.realtime import set_realtime_priority from common.transformations.model import model_height from common.transformations.camera import get_view_frame_from_road_frame from common.transformations.orientation import rot_from_euler, euler_from_rot -from selfdrive.hardware import TICI from selfdrive.swaglog import cloudlog MIN_SPEED_FILTER = 15 * CV.MPH_TO_MS @@ -68,7 +67,7 @@ class Calibrator: # Read saved calibration params = Params() calibration_params = params.get("CalibrationParams") - self.wide_camera = TICI and params.get_bool('EnableWideCamera') + self.wide_camera = params.get_bool('WideCameraOnly') rpy_init = RPY_INIT valid_blocks = 0 diff --git a/selfdrive/loggerd/bootlog.cc b/selfdrive/loggerd/bootlog.cc index eee6b86720..882b749fe9 100644 --- a/selfdrive/loggerd/bootlog.cc +++ b/selfdrive/loggerd/bootlog.cc @@ -8,7 +8,7 @@ static kj::Array build_boot_log() { std::vector bootlog_commands; - if (Hardware::TICI()) { + if (Hardware::AGNOS()) { bootlog_commands.push_back("journalctl"); bootlog_commands.push_back("sudo nvme smart-log --output-format=json /dev/nvme0"); } diff --git a/selfdrive/loggerd/encoder/encoder.h b/selfdrive/loggerd/encoder/encoder.h index 9792d61c46..312b68ba19 100644 --- a/selfdrive/loggerd/encoder/encoder.h +++ b/selfdrive/loggerd/encoder/encoder.h @@ -19,8 +19,7 @@ public: : filename(filename), type(type), in_width(in_width), in_height(in_height), fps(fps), bitrate(bitrate), codec(codec), out_width(out_width), out_height(out_height), write(write) { } virtual ~VideoEncoder(); - virtual int encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, - int in_width, int in_height, VisionIpcBufExtra *extra) = 0; + virtual int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) = 0; virtual void encoder_open(const char* path) = 0; virtual void encoder_close() = 0; diff --git a/selfdrive/loggerd/encoder/ffmpeg_encoder.cc b/selfdrive/loggerd/encoder/ffmpeg_encoder.cc index a2b14ef3fd..22587819a4 100644 --- a/selfdrive/loggerd/encoder/ffmpeg_encoder.cc +++ b/selfdrive/loggerd/encoder/ffmpeg_encoder.cc @@ -34,6 +34,8 @@ void FfmpegEncoder::encoder_init() { frame->linesize[1] = out_width/2; frame->linesize[2] = out_width/2; + convert_buf.resize(in_width * in_height * 3 / 2); + if (in_width != out_width || in_height != out_height) { downscale_buf.resize(out_width * out_height * 3 / 2); } @@ -70,18 +72,27 @@ void FfmpegEncoder::encoder_close() { is_open = false; } -int FfmpegEncoder::encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, - int in_width_, int in_height_, VisionIpcBufExtra *extra) { - assert(in_width_ == this->in_width); - assert(in_height_ == this->in_height); +int FfmpegEncoder::encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) { + assert(buf->width == this->in_width); + assert(buf->height == this->in_height); + + uint8_t *cy = convert_buf.data(); + uint8_t *cu = cy + in_width * in_height; + uint8_t *cv = cu + (in_width / 2) * (in_height / 2); + libyuv::NV12ToI420(buf->y, buf->stride, + buf->uv, buf->stride, + cy, in_width, + cu, in_width/2, + cv, in_width/2, + in_width, in_height); if (downscale_buf.size() > 0) { uint8_t *out_y = downscale_buf.data(); uint8_t *out_u = out_y + frame->width * frame->height; uint8_t *out_v = out_u + (frame->width / 2) * (frame->height / 2); - libyuv::I420Scale(y_ptr, in_width, - u_ptr, in_width/2, - v_ptr, in_width/2, + libyuv::I420Scale(cy, in_width, + cu, in_width/2, + cv, in_width/2, in_width, in_height, out_y, frame->width, out_u, frame->width/2, @@ -92,9 +103,9 @@ int FfmpegEncoder::encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, cons frame->data[1] = out_u; frame->data[2] = out_v; } else { - frame->data[0] = (uint8_t*)y_ptr; - frame->data[1] = (uint8_t*)u_ptr; - frame->data[2] = (uint8_t*)v_ptr; + frame->data[0] = cy; + frame->data[1] = cu; + frame->data[2] = cv; } frame->pts = counter*50*1000; // 50ms per frame diff --git a/selfdrive/loggerd/encoder/ffmpeg_encoder.h b/selfdrive/loggerd/encoder/ffmpeg_encoder.h index ae41abc69f..497a28b651 100644 --- a/selfdrive/loggerd/encoder/ffmpeg_encoder.h +++ b/selfdrive/loggerd/encoder/ffmpeg_encoder.h @@ -21,8 +21,7 @@ class FfmpegEncoder : public VideoEncoder { VideoEncoder(filename, type, in_width, in_height, fps, bitrate, cereal::EncodeIndex::Type::BIG_BOX_LOSSLESS, out_width, out_height, write) { encoder_init(); } ~FfmpegEncoder(); void encoder_init(); - int encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, - int in_width_, int in_height_, VisionIpcBufExtra *extra); + int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra); void encoder_open(const char* path); void encoder_close(); @@ -33,5 +32,6 @@ private: AVCodecContext *codec_ctx; AVFrame *frame = NULL; + std::vector convert_buf; std::vector downscale_buf; }; diff --git a/selfdrive/loggerd/encoder/v4l_encoder.cc b/selfdrive/loggerd/encoder/v4l_encoder.cc index 5fb9cc8de7..b3bd692b16 100644 --- a/selfdrive/loggerd/encoder/v4l_encoder.cc +++ b/selfdrive/loggerd/encoder/v4l_encoder.cc @@ -248,7 +248,6 @@ void V4LEncoder::encoder_init() { } // queue up input buffers for (unsigned int i = 0; i < BUF_IN_COUNT; i++) { - buf_in[i].allocate(fmt_in.fmt.pix_mp.plane_fmt[0].sizeimage); free_buf_in.push(i); } @@ -262,41 +261,19 @@ void V4LEncoder::encoder_open(const char* path) { this->counter = 0; } -int V4LEncoder::encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, - int in_width_, int in_height_, VisionIpcBufExtra *extra) { - assert(in_width == in_width_); - assert(in_height == in_height_); - assert(is_open); - - // reserve buffer - int buffer_in = free_buf_in.pop(); - - uint8_t *in_y_ptr = (uint8_t*)buf_in[buffer_in].addr; - int in_y_stride = VENUS_Y_STRIDE(COLOR_FMT_NV12, in_width); - int in_uv_stride = VENUS_UV_STRIDE(COLOR_FMT_NV12, in_width); - uint8_t *in_uv_ptr = in_y_ptr + (in_y_stride * VENUS_Y_SCANLINES(COLOR_FMT_NV12, in_height)); - - - // GRRR COPY - int err = libyuv::I420ToNV12(y_ptr, in_width, - u_ptr, in_width/2, - v_ptr, in_width/2, - in_y_ptr, in_y_stride, - in_uv_ptr, in_uv_stride, - in_width, in_height); - assert(err == 0); - +int V4LEncoder::encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) { struct timeval timestamp { .tv_sec = (long)(extra->timestamp_eof/1000000000), .tv_usec = (long)((extra->timestamp_eof/1000) % 1000000), }; + // reserve buffer + int buffer_in = free_buf_in.pop(); + // push buffer extras.push(*extra); - buf_in[buffer_in].sync(VISIONBUF_SYNC_TO_DEVICE); - int bytesused = VENUS_Y_STRIDE(COLOR_FMT_NV12, in_width) * VENUS_Y_SCANLINES(COLOR_FMT_NV12, in_height) + - VENUS_UV_STRIDE(COLOR_FMT_NV12, in_width) * VENUS_UV_SCANLINES(COLOR_FMT_NV12, in_height); - queue_buffer(fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, buffer_in, &buf_in[buffer_in], timestamp, bytesused); + //buf->sync(VISIONBUF_SYNC_TO_DEVICE); + queue_buffer(fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, buffer_in, buf, timestamp); return this->counter++; } diff --git a/selfdrive/loggerd/encoder/v4l_encoder.h b/selfdrive/loggerd/encoder/v4l_encoder.h index 9f267cfb03..b7c378be85 100644 --- a/selfdrive/loggerd/encoder/v4l_encoder.h +++ b/selfdrive/loggerd/encoder/v4l_encoder.h @@ -13,8 +13,7 @@ public: VideoEncoder(filename, type, in_width, in_height, fps, bitrate, codec, out_width, out_height, write) { encoder_init(); } ~V4LEncoder(); void encoder_init(); - int encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, - int in_width, int in_height, VisionIpcBufExtra *extra); + int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra); void encoder_open(const char* path); void encoder_close(); private: diff --git a/selfdrive/loggerd/encoderd.cc b/selfdrive/loggerd/encoderd.cc index 297eaf88d0..87cf4a492f 100644 --- a/selfdrive/loggerd/encoderd.cc +++ b/selfdrive/loggerd/encoderd.cc @@ -103,8 +103,7 @@ void encoder_thread(EncoderdState *s, const LogCameraInfo &cam_info) { // encode a frame for (int i = 0; i < encoders.size(); ++i) { - int out_id = encoders[i]->encode_frame(buf->y, buf->u, buf->v, - buf->width, buf->height, &extra); + int out_id = encoders[i]->encode_frame(buf, &extra); if (out_id == -1) { LOGE("Failed to encode frame. frame_id: %d", extra.frame_id); @@ -134,7 +133,7 @@ void encoderd_thread() { } int main() { - if (Hardware::TICI()) { + if (!Hardware::PC()) { int ret; ret = util::set_realtime_priority(52); assert(ret == 0); diff --git a/selfdrive/loggerd/logger.cc b/selfdrive/loggerd/logger.cc index d8a409e93e..5aed47e291 100644 --- a/selfdrive/loggerd/logger.cc +++ b/selfdrive/loggerd/logger.cc @@ -33,12 +33,7 @@ kj::Array logger_build_init_data() { MessageBuilder msg; auto init = msg.initEvent().initInitData(); - if (Hardware::TICI()) { - init.setDeviceType(cereal::InitData::DeviceType::TICI); - } else { - init.setDeviceType(cereal::InitData::DeviceType::PC); - } - + init.setDeviceType(Hardware::get_device_type()); init.setVersion(COMMA_VERSION); std::ifstream cmdline_stream("/proc/cmdline"); diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc index 1a10ab0c07..4086f4991e 100644 --- a/selfdrive/loggerd/loggerd.cc +++ b/selfdrive/loggerd/loggerd.cc @@ -267,7 +267,7 @@ void loggerd_thread() { } int main(int argc, char** argv) { - if (Hardware::TICI()) { + if (!Hardware::PC()) { int ret; ret = util::set_core_affinity({0, 1, 2, 3}); assert(ret == 0); @@ -279,4 +279,4 @@ int main(int argc, char** argv) { loggerd_thread(); return 0; -} \ No newline at end of file +} diff --git a/selfdrive/loggerd/loggerd.h b/selfdrive/loggerd/loggerd.h index 21aa66ea12..3b9b01a0fc 100644 --- a/selfdrive/loggerd/loggerd.h +++ b/selfdrive/loggerd/loggerd.h @@ -33,8 +33,8 @@ #endif constexpr int MAIN_FPS = 20; -const int MAIN_BITRATE = Hardware::TICI() ? 10000000 : 5000000; -const int DCAM_BITRATE = Hardware::TICI() ? MAIN_BITRATE : 2500000; +const int MAIN_BITRATE = 10000000; +const int DCAM_BITRATE = MAIN_BITRATE; #define NO_CAMERA_PATIENCE 500 // fall back to time-based rotation if all cameras are dead @@ -89,8 +89,8 @@ const LogCameraInfo cameras_logged[] = { .bitrate = MAIN_BITRATE, .is_h265 = true, .has_qcamera = false, - .enable = Hardware::TICI(), - .record = Hardware::TICI(), + .enable = true, + .record = true, .frame_width = 1928, .frame_height = 1208, }, @@ -102,6 +102,6 @@ const LogCameraInfo qcam_info = { .is_h265 = false, .enable = true, .record = true, - .frame_width = Hardware::TICI() ? 526 : 480, - .frame_height = Hardware::TICI() ? 330 : 360 // keep pixel count the same? + .frame_width = 526, + .frame_height = 330, }; diff --git a/selfdrive/loggerd/tests/test_loggerd.py b/selfdrive/loggerd/tests/test_loggerd.py index 89fb0c5838..54f7aaaa2b 100755 --- a/selfdrive/loggerd/tests/test_loggerd.py +++ b/selfdrive/loggerd/tests/test_loggerd.py @@ -15,14 +15,12 @@ from cereal.services import service_list from common.basedir import BASEDIR from common.params import Params from common.timeout import Timeout -from selfdrive.hardware import TICI from selfdrive.loggerd.config import ROOT from selfdrive.manager.process_config import managed_processes from selfdrive.version import get_version from tools.lib.logreader import LogReader from cereal.visionipc import VisionIpcServer, VisionStreamType -from common.transformations.camera import eon_f_frame_size, tici_f_frame_size, \ - eon_d_frame_size, tici_d_frame_size, tici_e_frame_size +from common.transformations.camera import tici_f_frame_size, tici_d_frame_size, tici_e_frame_size SentinelType = log.Sentinel.SentinelType @@ -108,17 +106,15 @@ class TestLoggerd(unittest.TestCase): os.environ["LOGGERD_TEST"] = "1" Params().put("RecordFront", "1") - expected_files = {"rlog", "qlog", "qcamera.ts", "fcamera.hevc", "dcamera.hevc"} - streams = [(VisionStreamType.VISION_STREAM_ROAD, tici_f_frame_size if TICI else eon_f_frame_size, "roadCameraState"), - (VisionStreamType.VISION_STREAM_DRIVER, tici_d_frame_size if TICI else eon_d_frame_size, "driverCameraState")] - if TICI: - expected_files.add("ecamera.hevc") - streams.append((VisionStreamType.VISION_STREAM_WIDE_ROAD, tici_e_frame_size, "wideRoadCameraState")) + expected_files = {"rlog", "qlog", "qcamera.ts", "fcamera.hevc", "dcamera.hevc", "ecamera.hevc"} + streams = [(VisionStreamType.VISION_STREAM_ROAD, (*tici_f_frame_size, 2048*2346, 2048, 2048*1216), "roadCameraState"), + (VisionStreamType.VISION_STREAM_DRIVER, (*tici_d_frame_size, 2048*2346, 2048, 2048*1216), "driverCameraState"), + (VisionStreamType.VISION_STREAM_WIDE_ROAD, (*tici_e_frame_size, 2048*2346, 2048, 2048*1216), "wideRoadCameraState")] pm = messaging.PubMaster(["roadCameraState", "driverCameraState", "wideRoadCameraState"]) vipc_server = VisionIpcServer("camerad") - for stream_type, frame_size, _ in streams: - vipc_server.create_buffers(stream_type, 40, False, *(frame_size)) + for stream_type, frame_spec, _ in streams: + vipc_server.create_buffers_with_sizes(stream_type, 40, False, *(frame_spec)) vipc_server.start_listener() for _ in range(5): @@ -130,8 +126,8 @@ class TestLoggerd(unittest.TestCase): fps = 20.0 for n in range(1, int(num_segs*length*fps)+1): - for stream_type, frame_size, state in streams: - dat = np.empty(int(frame_size[0]*frame_size[1]*3/2), dtype=np.uint8) + for stream_type, frame_spec, state in streams: + dat = np.empty(frame_spec[2], dtype=np.uint8) vipc_server.send(stream_type, dat[:].flatten().tobytes(), n, n/fps, n/fps) camera_state = messaging.new_message(state) diff --git a/selfdrive/manager/build.py b/selfdrive/manager/build.py index 806aa58bb7..bcc62a2932 100755 --- a/selfdrive/manager/build.py +++ b/selfdrive/manager/build.py @@ -10,12 +10,12 @@ from pathlib import Path from common.basedir import BASEDIR from common.spinner import Spinner from common.text_window import TextWindow -from selfdrive.hardware import TICI +from selfdrive.hardware import AGNOS from selfdrive.swaglog import cloudlog, add_file_handler from selfdrive.version import is_dirty MAX_CACHE_SIZE = 4e9 if "CI" in os.environ else 2e9 -CACHE_DIR = Path("/data/scons_cache" if TICI else "/tmp/scons_cache") +CACHE_DIR = Path("/data/scons_cache" if AGNOS else "/tmp/scons_cache") TOTAL_SCONS_NODES = 2405 MAX_BUILD_PROGRESS = 100 diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index ad3cabbbd9..48524aec1d 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -2,7 +2,7 @@ import os from cereal import car from common.params import Params -from selfdrive.hardware import TICI, PC +from selfdrive.hardware import PC from selfdrive.manager.process import PythonProcess, NativeProcess, DaemonProcess WEBCAM = os.getenv("USE_WEBCAM") is not None @@ -27,11 +27,10 @@ procs = [ NativeProcess("encoderd", "selfdrive/loggerd", ["./encoderd"]), NativeProcess("loggerd", "selfdrive/loggerd", ["./loggerd"], onroad=False, callback=logging), NativeProcess("modeld", "selfdrive/modeld", ["./modeld"]), - NativeProcess("navd", "selfdrive/ui/navd", ["./navd"], offroad=True), NativeProcess("proclogd", "selfdrive/proclogd", ["./proclogd"]), NativeProcess("sensord", "selfdrive/sensord", ["./sensord"], enabled=not PC), NativeProcess("ubloxd", "selfdrive/locationd", ["./ubloxd"], enabled=(not PC or WEBCAM)), - NativeProcess("ui", "selfdrive/ui", ["./ui"], offroad=True, watchdog_max_dt=(5 if TICI else None)), + NativeProcess("ui", "selfdrive/ui", ["./ui"], offroad=True, watchdog_max_dt=(5 if not PC else None)), NativeProcess("soundd", "selfdrive/ui/soundd", ["./soundd"], offroad=True), NativeProcess("locationd", "selfdrive/locationd", ["./locationd"]), NativeProcess("boardd", "selfdrive/boardd", ["./boardd"], enabled=False), @@ -40,12 +39,13 @@ procs = [ PythonProcess("deleter", "selfdrive.loggerd.deleter", offroad=True), PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", enabled=(not PC or WEBCAM), callback=driverview), PythonProcess("logmessaged", "selfdrive.logmessaged", offroad=True), + PythonProcess("navd", "selfdrive.navd.navd"), PythonProcess("pandad", "selfdrive.boardd.pandad", offroad=True), PythonProcess("paramsd", "selfdrive.locationd.paramsd"), PythonProcess("plannerd", "selfdrive.controls.plannerd"), PythonProcess("radard", "selfdrive.controls.radard"), PythonProcess("thermald", "selfdrive.thermald.thermald", offroad=True), - PythonProcess("timezoned", "selfdrive.timezoned", enabled=TICI, offroad=True), + PythonProcess("timezoned", "selfdrive.timezoned", enabled=not PC, offroad=True), PythonProcess("tombstoned", "selfdrive.tombstoned", enabled=not PC, offroad=True), PythonProcess("updated", "selfdrive.updated", enabled=not PC, onroad=False, offroad=True), PythonProcess("uploader", "selfdrive.loggerd.uploader", offroad=True), diff --git a/selfdrive/manager/test/test_manager.py b/selfdrive/manager/test/test_manager.py index 1750c81b2e..31949de0b9 100755 --- a/selfdrive/manager/test/test_manager.py +++ b/selfdrive/manager/test/test_manager.py @@ -5,7 +5,7 @@ import time import unittest import selfdrive.manager.manager as manager -from selfdrive.hardware import TICI, HARDWARE +from selfdrive.hardware import AGNOS, HARDWARE from selfdrive.manager.process import DaemonProcess from selfdrive.manager.process_config import managed_processes @@ -50,7 +50,7 @@ class TestManager(unittest.TestCase): self.assertTrue(state.running, f"{p} not running") exit_code = managed_processes[p].stop(retry=False) - if (TICI and p in ['ui', 'navd']): + if (AGNOS and p in ['ui',]): # TODO: make Qt UI exit gracefully continue diff --git a/selfdrive/modeld/dmonitoringmodeld.cc b/selfdrive/modeld/dmonitoringmodeld.cc index 949747047f..d0c33baf4e 100644 --- a/selfdrive/modeld/dmonitoringmodeld.cc +++ b/selfdrive/modeld/dmonitoringmodeld.cc @@ -31,7 +31,7 @@ void run_model(DMonitoringModelState &model, VisionIpcClient &vipc_client) { } double t1 = millis_since_boot(); - DMonitoringModelResult model_res = dmonitoring_eval_frame(&model, buf->addr, buf->width, buf->height, calib); + DMonitoringModelResult model_res = dmonitoring_eval_frame(&model, buf->addr, buf->width, buf->height, buf->stride, buf->uv_offset, calib); double t2 = millis_since_boot(); // send dm packet diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index 03d0ec4325..10cc2fe56e 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -163,7 +163,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl } int main(int argc, char **argv) { - if (Hardware::TICI()) { + if (!Hardware::PC()) { int ret; ret = util::set_realtime_priority(54); assert(ret == 0); @@ -171,7 +171,7 @@ int main(int argc, char **argv) { assert(ret == 0); } - bool main_wide_camera = Params().getBool("EnableWideCamera"); + bool main_wide_camera = Params().getBool("WideCameraOnly"); bool use_extra_client = !main_wide_camera; // set for single camera mode // cl init diff --git a/selfdrive/modeld/models/commonmodel.cc b/selfdrive/modeld/models/commonmodel.cc index bc013395df..b7c9051c6e 100644 --- a/selfdrive/modeld/models/commonmodel.cc +++ b/selfdrive/modeld/models/commonmodel.cc @@ -22,9 +22,9 @@ ModelFrame::ModelFrame(cl_device_id device_id, cl_context context) { loadyuv_init(&loadyuv, context, device_id, MODEL_WIDTH, MODEL_HEIGHT); } -float* ModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, const mat3 &projection, cl_mem *output) { +float* ModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3 &projection, cl_mem *output) { transform_queue(&this->transform, q, - yuv_cl, frame_width, frame_height, + yuv_cl, frame_width, frame_height, frame_stride, frame_uv_offset, y_cl, u_cl, v_cl, MODEL_WIDTH, MODEL_HEIGHT, projection); if (output == NULL) { diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h index 059a85e7dd..40c82a8c21 100644 --- a/selfdrive/modeld/models/commonmodel.h +++ b/selfdrive/modeld/models/commonmodel.h @@ -25,7 +25,7 @@ class ModelFrame { public: ModelFrame(cl_device_id device_id, cl_context context); ~ModelFrame(); - float* prepare(cl_mem yuv_cl, int width, int height, const mat3& transform, cl_mem *output); + float* prepare(cl_mem yuv_cl, int width, int height, int frame_stride, int frame_uv_offset, const mat3& transform, cl_mem *output); const int MODEL_WIDTH = 512; const int MODEL_HEIGHT = 256; diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc index ec1c6b811a..887998ca04 100644 --- a/selfdrive/modeld/models/dmonitoring.cc +++ b/selfdrive/modeld/models/dmonitoring.cc @@ -71,26 +71,21 @@ void fill_driver_data(cereal::DriverState::DriverData::Builder ddata, const Driv ddata.setNotReadyProb(ds_res.not_ready_prob); } -DMonitoringModelResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, float *calib) { +DMonitoringModelResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, int stride, int uv_offset, float *calib) { int v_off = height - MODEL_HEIGHT; int h_off = (width - MODEL_WIDTH) / 2; - int yuv_buf_len = (MODEL_WIDTH/2) * (MODEL_HEIGHT/2) * 6; // Y|u|v, frame2tensor done in dsp + int yuv_buf_len = MODEL_WIDTH * MODEL_HEIGHT; uint8_t *raw_buf = (uint8_t *) stream_buf; // vertical crop free uint8_t *raw_y_start = raw_buf + width * v_off; - uint8_t *raw_u_start = raw_buf + width * height + (width / 2) * (v_off / 2); - uint8_t *raw_v_start = raw_u_start + (width / 2) * (height / 2); float *net_input_buf = get_buffer(s->net_input_buf, yuv_buf_len); // snpe UserBufferEncodingUnsigned8Bit doesn't work // fast float conversion instead, also does h crop and scales to 0-1 - for (int r = 0; r < MODEL_HEIGHT / 2; ++r) { - libyuv::ByteToFloat(raw_y_start + (2*r) * width + h_off, net_input_buf + (2*r) * MODEL_WIDTH, 0.003921569f, MODEL_WIDTH); - libyuv::ByteToFloat(raw_y_start + (2*r+1) * width + h_off, net_input_buf + (2*r+1) * MODEL_WIDTH, 0.003921569f, MODEL_WIDTH); - libyuv::ByteToFloat(raw_u_start + r * (width / 2) + (h_off / 2), net_input_buf + MODEL_WIDTH * MODEL_HEIGHT + r * MODEL_WIDTH / 2, 0.003921569f, MODEL_WIDTH / 2); - libyuv::ByteToFloat(raw_v_start + r * (width / 2) + (h_off / 2), net_input_buf + MODEL_WIDTH * MODEL_HEIGHT * 5 / 4 + r * MODEL_WIDTH / 2, 0.003921569f, MODEL_WIDTH / 2); + for (int r = 0; r < MODEL_HEIGHT; ++r) { + libyuv::ByteToFloat(raw_y_start + r * width + h_off, net_input_buf + r * MODEL_WIDTH, 0.003921569f, MODEL_WIDTH); } // printf("preprocess completed. %d \n", yuv_buf_len); diff --git a/selfdrive/modeld/models/dmonitoring.h b/selfdrive/modeld/models/dmonitoring.h index 7aa6dad7cb..1a6e825e9b 100644 --- a/selfdrive/modeld/models/dmonitoring.h +++ b/selfdrive/modeld/models/dmonitoring.h @@ -46,7 +46,7 @@ typedef struct DMonitoringModelState { } DMonitoringModelState; void dmonitoring_init(DMonitoringModelState* s); -DMonitoringModelResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, float *calib); +DMonitoringModelResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, int stride, int uv_offset, float *calib); void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringModelResult &model_res, float execution_time, kj::ArrayPtr raw_pred); void dmonitoring_free(DMonitoringModelState* s); diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index b601c4898b..59c0b249d9 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -73,12 +73,12 @@ ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf, #endif // if getInputBuf is not NULL, net_input_buf will be - auto net_input_buf = s->frame->prepare(buf->buf_cl, buf->width, buf->height, transform, static_cast(s->m->getInputBuf())); + auto net_input_buf = s->frame->prepare(buf->buf_cl, buf->width, buf->height, buf->stride, buf->uv_offset, transform, static_cast(s->m->getInputBuf())); s->m->addImage(net_input_buf, s->frame->buf_size); LOGT("Image added"); if (wbuf != nullptr) { - auto net_extra_buf = s->wide_frame->prepare(wbuf->buf_cl, wbuf->width, wbuf->height, transform_wide, static_cast(s->m->getExtraBuf())); + auto net_extra_buf = s->wide_frame->prepare(wbuf->buf_cl, wbuf->width, wbuf->height, wbuf->stride, wbuf->uv_offset, transform_wide, static_cast(s->m->getExtraBuf())); s->m->addExtra(net_extra_buf, s->wide_frame->buf_size); LOGT("Extra image added"); } diff --git a/selfdrive/modeld/transforms/transform.cc b/selfdrive/modeld/transforms/transform.cc index 8929f56a9e..cabc58a46d 100644 --- a/selfdrive/modeld/transforms/transform.cc +++ b/selfdrive/modeld/transforms/transform.cc @@ -25,7 +25,7 @@ void transform_destroy(Transform* s) { void transform_queue(Transform* s, cl_command_queue q, - cl_mem in_yuv, int in_width, int in_height, + cl_mem in_yuv, int in_width, int in_height, int in_stride, int in_uv_offset, cl_mem out_y, cl_mem out_u, cl_mem out_v, int out_width, int out_height, const mat3& projection) { @@ -44,28 +44,30 @@ void transform_queue(Transform* s, const int in_y_width = in_width; const int in_y_height = in_height; + const int in_y_px_stride = 1; const int in_uv_width = in_width/2; const int in_uv_height = in_height/2; - const int in_y_offset = 0; - const int in_u_offset = in_y_offset + in_y_width*in_y_height; - const int in_v_offset = in_u_offset + in_uv_width*in_uv_height; + const int in_uv_px_stride = 2; + const int in_u_offset = in_uv_offset; + const int in_v_offset = in_uv_offset + 1; const int out_y_width = out_width; const int out_y_height = out_height; const int out_uv_width = out_width/2; const int out_uv_height = out_height/2; - CL_CHECK(clSetKernelArg(s->krnl, 0, sizeof(cl_mem), &in_yuv)); - CL_CHECK(clSetKernelArg(s->krnl, 1, sizeof(cl_int), &in_y_width)); - CL_CHECK(clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_y_offset)); - CL_CHECK(clSetKernelArg(s->krnl, 3, sizeof(cl_int), &in_y_height)); - CL_CHECK(clSetKernelArg(s->krnl, 4, sizeof(cl_int), &in_y_width)); - CL_CHECK(clSetKernelArg(s->krnl, 5, sizeof(cl_mem), &out_y)); - CL_CHECK(clSetKernelArg(s->krnl, 6, sizeof(cl_int), &out_y_width)); - CL_CHECK(clSetKernelArg(s->krnl, 7, sizeof(cl_int), &zero)); - CL_CHECK(clSetKernelArg(s->krnl, 8, sizeof(cl_int), &out_y_height)); - CL_CHECK(clSetKernelArg(s->krnl, 9, sizeof(cl_int), &out_y_width)); - CL_CHECK(clSetKernelArg(s->krnl, 10, sizeof(cl_mem), &s->m_y_cl)); + CL_CHECK(clSetKernelArg(s->krnl, 0, sizeof(cl_mem), &in_yuv)); // src + CL_CHECK(clSetKernelArg(s->krnl, 1, sizeof(cl_int), &in_stride)); // src_row_stride + CL_CHECK(clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_y_px_stride)); // src_px_stride + CL_CHECK(clSetKernelArg(s->krnl, 3, sizeof(cl_int), &zero)); // src_offset + CL_CHECK(clSetKernelArg(s->krnl, 4, sizeof(cl_int), &in_y_height)); // src_rows + CL_CHECK(clSetKernelArg(s->krnl, 5, sizeof(cl_int), &in_y_width)); // src_cols + CL_CHECK(clSetKernelArg(s->krnl, 6, sizeof(cl_mem), &out_y)); // dst + CL_CHECK(clSetKernelArg(s->krnl, 7, sizeof(cl_int), &out_y_width)); // dst_row_stride + CL_CHECK(clSetKernelArg(s->krnl, 8, sizeof(cl_int), &zero)); // dst_offset + CL_CHECK(clSetKernelArg(s->krnl, 9, sizeof(cl_int), &out_y_height)); // dst_rows + CL_CHECK(clSetKernelArg(s->krnl, 10, sizeof(cl_int), &out_y_width)); // dst_cols + CL_CHECK(clSetKernelArg(s->krnl, 11, sizeof(cl_mem), &s->m_y_cl)); // M const size_t work_size_y[2] = {(size_t)out_y_width, (size_t)out_y_height}; @@ -74,21 +76,21 @@ void transform_queue(Transform* s, const size_t work_size_uv[2] = {(size_t)out_uv_width, (size_t)out_uv_height}; - CL_CHECK(clSetKernelArg(s->krnl, 1, sizeof(cl_int), &in_uv_width)); - CL_CHECK(clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_u_offset)); - CL_CHECK(clSetKernelArg(s->krnl, 3, sizeof(cl_int), &in_uv_height)); - CL_CHECK(clSetKernelArg(s->krnl, 4, sizeof(cl_int), &in_uv_width)); - CL_CHECK(clSetKernelArg(s->krnl, 5, sizeof(cl_mem), &out_u)); - CL_CHECK(clSetKernelArg(s->krnl, 6, sizeof(cl_int), &out_uv_width)); - CL_CHECK(clSetKernelArg(s->krnl, 7, sizeof(cl_int), &zero)); - CL_CHECK(clSetKernelArg(s->krnl, 8, sizeof(cl_int), &out_uv_height)); - CL_CHECK(clSetKernelArg(s->krnl, 9, sizeof(cl_int), &out_uv_width)); - CL_CHECK(clSetKernelArg(s->krnl, 10, sizeof(cl_mem), &s->m_uv_cl)); - + CL_CHECK(clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_uv_px_stride)); // src_px_stride + CL_CHECK(clSetKernelArg(s->krnl, 3, sizeof(cl_int), &in_u_offset)); // src_offset + CL_CHECK(clSetKernelArg(s->krnl, 4, sizeof(cl_int), &in_uv_height)); // src_rows + CL_CHECK(clSetKernelArg(s->krnl, 5, sizeof(cl_int), &in_uv_width)); // src_cols + CL_CHECK(clSetKernelArg(s->krnl, 6, sizeof(cl_mem), &out_u)); // dst + CL_CHECK(clSetKernelArg(s->krnl, 7, sizeof(cl_int), &out_uv_width)); // dst_row_stride + CL_CHECK(clSetKernelArg(s->krnl, 8, sizeof(cl_int), &zero)); // dst_offset + CL_CHECK(clSetKernelArg(s->krnl, 9, sizeof(cl_int), &out_uv_height)); // dst_rows + CL_CHECK(clSetKernelArg(s->krnl, 10, sizeof(cl_int), &out_uv_width)); // dst_cols + CL_CHECK(clSetKernelArg(s->krnl, 11, sizeof(cl_mem), &s->m_uv_cl)); // M + CL_CHECK(clEnqueueNDRangeKernel(q, s->krnl, 2, NULL, (const size_t*)&work_size_uv, NULL, 0, 0, NULL)); - CL_CHECK(clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_v_offset)); - CL_CHECK(clSetKernelArg(s->krnl, 5, sizeof(cl_mem), &out_v)); + CL_CHECK(clSetKernelArg(s->krnl, 3, sizeof(cl_int), &in_v_offset)); // src_ofset + CL_CHECK(clSetKernelArg(s->krnl, 6, sizeof(cl_mem), &out_v)); // dst CL_CHECK(clEnqueueNDRangeKernel(q, s->krnl, 2, NULL, (const size_t*)&work_size_uv, NULL, 0, 0, NULL)); diff --git a/selfdrive/modeld/transforms/transform.cl b/selfdrive/modeld/transforms/transform.cl index 8ad1869351..357ef87321 100644 --- a/selfdrive/modeld/transforms/transform.cl +++ b/selfdrive/modeld/transforms/transform.cl @@ -6,9 +6,9 @@ #define INTER_REMAP_COEF_SCALE (1 << INTER_REMAP_COEF_BITS) __kernel void warpPerspective(__global const uchar * src, - int src_step, int src_offset, int src_rows, int src_cols, + int src_row_stride, int src_px_stride, int src_offset, int src_rows, int src_cols, __global uchar * dst, - int dst_step, int dst_offset, int dst_rows, int dst_cols, + int dst_row_stride, int dst_offset, int dst_rows, int dst_cols, __constant float * M) { int dx = get_global_id(0); @@ -28,18 +28,18 @@ __kernel void warpPerspective(__global const uchar * src, short ax = (short)(X & (INTER_TAB_SIZE - 1)); int v0 = (sx >= 0 && sx < src_cols && sy >= 0 && sy < src_rows) ? - convert_int(src[mad24(sy, src_step, src_offset + sx)]) : 0; + convert_int(src[mad24(sy, src_row_stride, src_offset + sx*src_px_stride)]) : 0; int v1 = (sx+1 >= 0 && sx+1 < src_cols && sy >= 0 && sy < src_rows) ? - convert_int(src[mad24(sy, src_step, src_offset + (sx+1))]) : 0; + convert_int(src[mad24(sy, src_row_stride, src_offset + (sx+1)*src_px_stride)]) : 0; int v2 = (sx >= 0 && sx < src_cols && sy+1 >= 0 && sy+1 < src_rows) ? - convert_int(src[mad24(sy+1, src_step, src_offset + sx)]) : 0; + convert_int(src[mad24(sy+1, src_row_stride, src_offset + sx*src_px_stride)]) : 0; int v3 = (sx+1 >= 0 && sx+1 < src_cols && sy+1 >= 0 && sy+1 < src_rows) ? - convert_int(src[mad24(sy+1, src_step, src_offset + (sx+1))]) : 0; + convert_int(src[mad24(sy+1, src_row_stride, src_offset + (sx+1)*src_px_stride)]) : 0; float taby = 1.f/INTER_TAB_SIZE*ay; float tabx = 1.f/INTER_TAB_SIZE*ax; - int dst_index = mad24(dy, dst_step, dst_offset + dx); + int dst_index = mad24(dy, dst_row_stride, dst_offset + dx); int itab0 = convert_short_sat_rte( (1.0f-taby)*(1.0f-tabx) * INTER_REMAP_COEF_SCALE ); int itab1 = convert_short_sat_rte( (1.0f-taby)*tabx * INTER_REMAP_COEF_SCALE ); diff --git a/selfdrive/modeld/transforms/transform.h b/selfdrive/modeld/transforms/transform.h index a1629a517e..771a7054b3 100644 --- a/selfdrive/modeld/transforms/transform.h +++ b/selfdrive/modeld/transforms/transform.h @@ -19,7 +19,7 @@ void transform_init(Transform* s, cl_context ctx, cl_device_id device_id); void transform_destroy(Transform* transform); void transform_queue(Transform* s, cl_command_queue q, - cl_mem yuv, int in_width, int in_height, + cl_mem yuv, int in_width, int in_height, int in_stride, int in_uv_offset, cl_mem out_y, cl_mem out_u, cl_mem out_v, int out_width, int out_height, const mat3& projection); diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py index b4c1e020ed..f2fc26645d 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/selfdrive/monitoring/driver_monitor.py @@ -16,7 +16,7 @@ EventName = car.CarEvent.EventName # ****************************************************************************************** class DRIVER_MONITOR_SETTINGS(): - def __init__(self, DT_DMON=DT_DMON): + def __init__(self): self._DT_DMON = DT_DMON # ref (page15-16): https://eur-lex.europa.eu/legal-content/EN/TXT/PDF/?uri=CELEX:42018X1947&rid=2 self._AWARENESS_TIME = 30. # passive wheeltouch total timeout @@ -29,7 +29,7 @@ class DRIVER_MONITOR_SETTINGS(): self._FACE_THRESHOLD = 0.5 self._EYE_THRESHOLD = 0.65 self._SG_THRESHOLD = 0.925 - self._BLINK_THRESHOLD = 0.6 + self._BLINK_THRESHOLD = 0.8 self._EE_THRESH11 = 0.75 self._EE_THRESH12 = 3.25 diff --git a/selfdrive/navd/__init__.py b/selfdrive/navd/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/navd/helpers.py b/selfdrive/navd/helpers.py new file mode 100644 index 0000000000..89fd5e7e4e --- /dev/null +++ b/selfdrive/navd/helpers.py @@ -0,0 +1,162 @@ +from __future__ import annotations + +import json +import math +from typing import Any, Dict, List, Optional, Tuple + +from common.numpy_fast import clip +from common.params import Params + +EARTH_MEAN_RADIUS = 6371007.2 + + +class Coordinate: + def __init__(self, latitude: float, longitude: float) -> None: + self.latitude = latitude + self.longitude = longitude + + @classmethod + def from_mapbox_tuple(cls, t: Tuple[float, float]) -> Coordinate: + return cls(t[1], t[0]) + + def as_dict(self) -> Dict[str, float]: + return {'latitude': self.latitude, 'longitude': self.longitude} + + def __str__(self) -> str: + return f"({self.latitude}, {self.longitude})" + + def __eq__(self, other) -> bool: + if not isinstance(other, Coordinate): + return False + return (self.latitude == other.latitude) and (self.longitude == other.longitude) + + def __sub__(self, other: Coordinate) -> Coordinate: + return Coordinate(self.latitude - other.latitude, self.longitude - other.longitude) + + def __add__(self, other: Coordinate) -> Coordinate: + return Coordinate(self.latitude + other.latitude, self.longitude + other.longitude) + + def __mul__(self, c: float) -> Coordinate: + return Coordinate(self.latitude * c, self.longitude * c) + + def dot(self, other: Coordinate) -> float: + return self.latitude * other.latitude + self.longitude * other.longitude + + def distance_to(self, other: Coordinate) -> float: + # Haversine formula + dlat = math.radians(other.latitude - self.latitude) + dlon = math.radians(other.longitude - self.longitude) + + haversine_dlat = math.sin(dlat / 2.0) + haversine_dlat *= haversine_dlat + haversine_dlon = math.sin(dlon / 2.0) + haversine_dlon *= haversine_dlon + + y = haversine_dlat \ + + math.cos(math.radians(self.latitude)) \ + * math.cos(math.radians(other.latitude)) \ + * haversine_dlon + x = 2 * math.asin(math.sqrt(y)) + return x * EARTH_MEAN_RADIUS + + +def minimum_distance(a: Coordinate, b: Coordinate, p: Coordinate): + if a.distance_to(b) < 0.01: + return a.distance_to(p) + + ap = p - a + ab = b - a + t = clip(ap.dot(ab) / ab.dot(ab), 0.0, 1.0) + projection = a + ab * t + return projection.distance_to(p) + + +def distance_along_geometry(geometry: List[Coordinate], pos: Coordinate) -> float: + if len(geometry) <= 2: + return geometry[0].distance_to(pos) + + # 1. Find segment that is closest to current position + # 2. Total distance is sum of distance to start of closest segment + # + all previous segments + total_distance = 0.0 + total_distance_closest = 0.0 + closest_distance = 1e9 + + for i in range(len(geometry) - 1): + d = minimum_distance(geometry[i], geometry[i + 1], pos) + + if d < closest_distance: + closest_distance = d + total_distance_closest = total_distance + geometry[i].distance_to(pos) + + total_distance += geometry[i].distance_to(geometry[i + 1]) + + return total_distance_closest + + +def coordinate_from_param(param: str, params: Optional[Params] = None) -> Optional[Coordinate]: + if params is None: + params = Params() + + json_str = params.get(param) + if json_str is None: + return None + + pos = json.loads(json_str) + if 'latitude' not in pos or 'longitude' not in pos: + return None + + return Coordinate(pos['latitude'], pos['longitude']) + + +def string_to_direction(direction: str) -> str: + for d in ['left', 'right', 'straight']: + if d in direction: + return d + return 'none' + + +def parse_banner_instructions(instruction: Any, banners: Any, distance_to_maneuver: float = 0.0) -> None: + if not len(banners): + return + + current_banner = banners[0] + + # A segment can contain multiple banners, find one that we need to show now + for banner in banners: + if distance_to_maneuver < banner['distanceAlongGeometry']: + current_banner = banner + + # Only show banner when close enough to maneuver + instruction.showFull = distance_to_maneuver < current_banner['distanceAlongGeometry'] + + # Primary + p = current_banner['primary'] + if 'text' in p: + instruction.maneuverPrimaryText = p['text'] + if 'type' in p: + instruction.maneuverType = p['type'] + if 'modifier' in p: + instruction.maneuverModifier = p['modifier'] + + # Secondary + if 'secondary' in current_banner: + instruction.maneuverSecondaryText = current_banner['secondary']['text'] + + # Lane lines + if 'sub' in current_banner: + lanes = [] + for component in current_banner['sub']['components']: + if component['type'] != 'lane': + continue + + lane = { + 'active': component['active'], + 'directions': [string_to_direction(d) for d in component['directions']], + } + + if 'active_direction' in component: + lane['activeDirection'] = string_to_direction(component['active_direction']) + + lanes.append(lane) + instruction.lanes = lanes diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py new file mode 100755 index 0000000000..0a8eab7e06 --- /dev/null +++ b/selfdrive/navd/navd.py @@ -0,0 +1,255 @@ +#!/usr/bin/env python3 +import math +import os +import threading + +import requests + +import cereal.messaging as messaging +from cereal import log +from common.api import Api +from common.params import Params +from common.realtime import Ratekeeper +from selfdrive.swaglog import cloudlog +from selfdrive.navd.helpers import (Coordinate, coordinate_from_param, + distance_along_geometry, + minimum_distance, + parse_banner_instructions) + +REROUTE_DISTANCE = 25 +MANEUVER_TRANSITION_THRESHOLD = 10 + + +class RouteEngine: + def __init__(self, sm, pm): + self.sm = sm + self.pm = pm + + self.params = Params() + + # Get last gps position from params + self.last_position = coordinate_from_param("LastGPSPosition", self.params) + self.last_bearing = None + + self.gps_ok = False + + self.nav_destination = None + self.step_idx = None + self.route = None + self.route_geometry = None + + self.recompute_backoff = 0 + self.recompute_countdown = 0 + + self.ui_pid = None + + if "MAPBOX_TOKEN" in os.environ: + self.mapbox_token = os.environ["MAPBOX_TOKEN"] + self.mapbox_host = "https://api.mapbox.com" + else: + try: + self.mapbox_token = Api(self.params.get("DongleId", encoding='utf8')).get_token(expiry_hours=4 * 7 * 24) + except FileNotFoundError: + cloudlog.exception("Failed to generate mapbox token due to missing private key. Ensure device is registered.") + self.mapbox_token = "" + self.mapbox_host = "https://maps.comma.ai" + + def update(self): + self.sm.update(0) + + if self.sm.updated["managerState"]: + ui_pid = [p.pid for p in self.sm["managerState"].processes if p.name == "ui" and p.running] + if ui_pid: + if self.ui_pid and self.ui_pid != ui_pid[0]: + cloudlog.warning("UI restarting, sending route") + threading.Timer(5.0, self.send_route).start() + self.ui_pid = ui_pid[0] + + self.update_location() + self.recompute_route() + self.send_instruction() + + def update_location(self): + location = self.sm['liveLocationKalman'] + self.gps_ok = location.gpsOK + + localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid + + if localizer_valid: + self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2]) + self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1]) + + def recompute_route(self): + if self.last_position is None: + return + + new_destination = coordinate_from_param("NavDestination", self.params) + if new_destination is None: + self.clear_route() + return + + should_recompute = self.should_recompute() + if new_destination != self.nav_destination: + cloudlog.warning(f"Got new destination from NavDestination param {new_destination}") + should_recompute = True + + # Don't recompute when GPS drifts in tunnels + if not self.gps_ok and self.step_idx is not None: + return + + if self.recompute_countdown == 0 and should_recompute: + self.recompute_countdown = 2**self.recompute_backoff + self.recompute_backoff = min(6, self.recompute_backoff + 1) + self.calculate_route(new_destination) + else: + self.recompute_countdown = max(0, self.recompute_countdown - 1) + + def calculate_route(self, destination): + cloudlog.warning(f"Calculating route {self.last_position} -> {destination}") + self.nav_destination = destination + + params = { + 'access_token': self.mapbox_token, + # 'annotations': 'maxspeed', + 'geometries': 'geojson', + 'overview': 'full', + 'steps': 'true', + 'banner_instructions': 'true', + 'alternatives': 'false', + } + + if self.last_bearing is not None: + params['bearings'] = f"{(self.last_bearing + 360) % 360:.0f},90;" + + url = self.mapbox_host + f'/directions/v5/mapbox/driving-traffic/{self.last_position.longitude},{self.last_position.latitude};{destination.longitude},{destination.latitude}' + try: + resp = requests.get(url, params=params) + resp.raise_for_status() + + r = resp.json() + if len(r['routes']): + self.route = r['routes'][0]['legs'][0]['steps'] + self.route_geometry = [] + + # Convert coordinates + for step in self.route: + self.route_geometry.append([Coordinate.from_mapbox_tuple(c) for c in step['geometry']['coordinates']]) + + self.step_idx = 0 + else: + cloudlog.warning("Got empty route response") + self.clear_route() + + except requests.exceptions.RequestException: + cloudlog.exception("failed to get route") + self.clear_route() + + self.send_route() + + def send_instruction(self): + msg = messaging.new_message('navInstruction') + + if self.step_idx is None: + msg.valid = False + self.pm.send('navInstruction', msg) + return + + step = self.route[self.step_idx] + geometry = self.route_geometry[self.step_idx] + along_geometry = distance_along_geometry(geometry, self.last_position) + distance_to_maneuver_along_geometry = step['distance'] - along_geometry + + # Current instruction + msg.navInstruction.maneuverDistance = distance_to_maneuver_along_geometry + parse_banner_instructions(msg.navInstruction, step['bannerInstructions'], distance_to_maneuver_along_geometry) + + # Compute total remaining time and distance + remaning = 1.0 - along_geometry / max(step['distance'], 1) + total_distance = step['distance'] * remaning + total_time = step['duration'] * remaning + total_time_typical = step['duration_typical'] * remaning + + # Add up totals for future steps + for i in range(self.step_idx + 1, len(self.route)): + total_distance += self.route[i]['distance'] + total_time += self.route[i]['duration'] + total_time_typical += self.route[i]['duration_typical'] + + msg.navInstruction.distanceRemaining = total_distance + msg.navInstruction.timeRemaining = total_time + msg.navInstruction.timeRemainingTypical = total_time_typical + + self.pm.send('navInstruction', msg) + + # Transition to next route segment + if distance_to_maneuver_along_geometry < -MANEUVER_TRANSITION_THRESHOLD: + if self.step_idx + 1 < len(self.route): + self.step_idx += 1 + self.recompute_backoff = 0 + self.recompute_countdown = 0 + else: + cloudlog.warning("Destination reached") + Params().delete("NavDestination") + + # Clear route if driving away from destination + dist = self.nav_destination.distance_to(self.last_position) + if dist > REROUTE_DISTANCE: + self.clear_route() + + def send_route(self): + coords = [] + + if self.route is not None: + for path in self.route_geometry: + coords += [c.as_dict() for c in path] + + msg = messaging.new_message('navRoute') + msg.navRoute.coordinates = coords + self.pm.send('navRoute', msg) + + def clear_route(self): + self.route = None + self.route_geometry = None + self.step_idx = None + self.nav_destination = None + + def should_recompute(self): + if self.step_idx is None or self.route is None: + return True + + # Don't recompute in last segment, assume destination is reached + if self.step_idx == len(self.route) - 1: + return False + + # Compute closest distance to all line segments in the current path + min_d = REROUTE_DISTANCE + 1 + path = self.route_geometry[self.step_idx] + for i in range(len(path) - 1): + a = path[i] + b = path[i + 1] + + if a.distance_to(b) < 1.0: + continue + + min_d = min(min_d, minimum_distance(a, b, self.last_position)) + + return min_d > REROUTE_DISTANCE + + # TODO: Check for going wrong way in segment + + +def main(sm=None, pm=None): + if sm is None: + sm = messaging.SubMaster(['liveLocationKalman', 'managerState']) + if pm is None: + pm = messaging.PubMaster(['navInstruction', 'navRoute']) + + rk = Ratekeeper(1.0) + route_engine = RouteEngine(sm, pm) + while True: + route_engine.update() + rk.keep_time() + + +if __name__ == "__main__": + main() diff --git a/selfdrive/test/process_replay/README.md b/selfdrive/test/process_replay/README.md index e760e4e135..9fa4ca644a 100644 --- a/selfdrive/test/process_replay/README.md +++ b/selfdrive/test/process_replay/README.md @@ -5,6 +5,7 @@ Process replay is a regression test designed to identify any changes in the outp If the test fails, make sure that you didn't unintentionally change anything. If there are intentional changes, the reference logs will be updated. Use `test_processes.py` to run the test locally. +Use `FILEREADER_CACHE='1' test_processes.py` to cache log files. Currently the following processes are tested: diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py index 20739ae37d..34f35e0ed7 100755 --- a/selfdrive/test/process_replay/model_replay.py +++ b/selfdrive/test/process_replay/model_replay.py @@ -96,7 +96,7 @@ def model_replay(lr, frs): if msg.which() in VIPC_STREAM: msg = msg.as_builder() camera_state = getattr(msg, msg.which()) - img = frs[msg.which()].get(frame_idxs[msg.which()], pix_fmt="yuv420p")[0] + img = frs[msg.which()].get(frame_idxs[msg.which()], pix_fmt="nv12")[0] frame_idxs[msg.which()] += 1 # send camera state and frame diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index c1428ccf96..1eda9bc7f5 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -242,14 +242,17 @@ CONFIGS = [ pub_sub={ "can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"], "deviceState": [], "pandaStates": [], "peripheralState": [], "liveCalibration": [], "driverMonitoringState": [], "longitudinalPlan": [], "lateralPlan": [], "liveLocationKalman": [], "liveParameters": [], "radarState": [], - "modelV2": [], "driverCameraState": [], "roadCameraState": [], "managerState": [], "testJoystick": [], + "modelV2": [], "driverCameraState": [], "roadCameraState": [], "wideRoadCameraState": [], "managerState": [], "testJoystick": [], }, ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"], init_callback=fingerprint, should_recv_callback=controlsd_rcv_callback, tolerance=NUMPY_TOLERANCE, fake_pubsubmaster=True, - submaster_config={'ignore_avg_freq': ['radarState', 'longitudinalPlan']} + submaster_config={ + 'ignore_avg_freq': ['radarState', 'longitudinalPlan', 'driverCameraState', 'driverMonitoringState'], # dcam is expected at 20 Hz + 'ignore_alive': ['wideRoadCameraState'], # TODO: Add to regen + } ), ProcessConfig( proc_name="radard", @@ -350,7 +353,7 @@ def setup_env(simulation=False): params.put_bool("OpenpilotEnabledToggle", True) params.put_bool("Passive", False) params.put_bool("DisengageOnAccelerator", True) - params.put_bool("EnableWideCamera", False) + params.put_bool("WideCameraOnly", False) params.put_bool("DisableLogging", False) os.environ["NO_RADAR_SLEEP"] = "1" diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index de8599a58e..502fa15f2a 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -0956446adfa91506f0a3d88f893e041bfb2890c1 +49e231ccf06ef35994a3ec907af959c28e3c0870 \ No newline at end of file diff --git a/selfdrive/test/process_replay/regen.py b/selfdrive/test/process_replay/regen.py index b0bc033fe6..bcc91b3ab6 100755 --- a/selfdrive/test/process_replay/regen.py +++ b/selfdrive/test/process_replay/regen.py @@ -164,7 +164,7 @@ def replay_cameras(lr, frs): print(f"Decompressing frames {s}") frames = [] for i in tqdm(range(fr.frame_count)): - img = fr.get(i, pix_fmt='yuv420p')[0] + img = fr.get(i, pix_fmt='nv12')[0] frames.append(img.flatten().tobytes()) vs.create_buffers(stream, 40, False, size[0], size[1]) diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index afab6cc765..1d5969b1ec 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -12,6 +12,7 @@ from selfdrive.test.openpilotci import get_url, upload_file from selfdrive.test.process_replay.compare_logs import compare_logs, save_log from selfdrive.test.process_replay.process_replay import CONFIGS, PROC_REPLAY_DIR, FAKEDATA, check_enabled, replay_process from selfdrive.version import get_commit +from tools.lib.filereader import FileReader from tools.lib.logreader import LogReader original_segments = [ @@ -57,7 +58,8 @@ REF_COMMIT_FN = os.path.join(PROC_REPLAY_DIR, "ref_commit") def run_test_process(data): - segment, cfg, args, cur_log_fn, ref_log_path, lr = data + segment, cfg, args, cur_log_fn, ref_log_path, lr_dat = data + lr = LogReader.from_bytes(lr_dat) res = None if not args.upload_only: res, log_msgs = test_process(cfg, lr, ref_log_path, args.ignore_fields, args.ignore_msgs) @@ -72,10 +74,10 @@ def run_test_process(data): return (segment, cfg.proc_name, res) -def get_logreader(segment): +def get_log_data(segment): r, n = segment.rsplit("--", 1) - lr = LogReader(get_url(r, n)) - return (segment, lr) + with FileReader(get_url(r, n)) as f: + return (segment, f.read()) def test_process(cfg, lr, ref_log_path, ignore_fields=None, ignore_msgs=None): @@ -131,12 +133,13 @@ def format_diff(results, ref_commit): if __name__ == "__main__": - parser = argparse.ArgumentParser(description="Regression test to identify changes in a process's output") + all_cars = {car for car, _ in segments} + all_procs = {cfg.proc_name for cfg in CONFIGS} - # whitelist has precedence over blacklist in case both are defined - parser.add_argument("--whitelist-procs", type=str, nargs="*", default=[], + parser = argparse.ArgumentParser(description="Regression test to identify changes in a process's output") + parser.add_argument("--whitelist-procs", type=str, nargs="*", default=all_procs, help="Whitelist given processes from the test (e.g. controlsd)") - parser.add_argument("--whitelist-cars", type=str, nargs="*", default=[], + parser.add_argument("--whitelist-cars", type=str, nargs="*", default=all_cars, help="Whitelist given cars from the test (e.g. HONDA)") parser.add_argument("--blacklist-procs", type=str, nargs="*", default=[], help="Blacklist given processes from the test (e.g. controlsd)") @@ -153,6 +156,10 @@ if __name__ == "__main__": parser.add_argument("-j", "--jobs", type=int, default=1) args = parser.parse_args() + tested_procs = set(args.whitelist_procs) - set(args.blacklist_procs) + tested_cars = set(args.whitelist_cars) - set(args.blacklist_cars) + tested_cars = {c.upper() for c in tested_cars} + full_test = all(len(x) == 0 for x in (args.whitelist_procs, args.whitelist_cars, args.blacklist_procs, args.blacklist_cars, args.ignore_fields, args.ignore_msgs)) upload = args.update_refs or args.upload_only os.makedirs(os.path.dirname(FAKEDATA), exist_ok=True) @@ -180,20 +187,19 @@ if __name__ == "__main__": with concurrent.futures.ProcessPoolExecutor(max_workers=args.jobs) as pool: if not args.upload_only: - lreaders: Any = {} - p1 = pool.map(get_logreader, [seg for car, seg in segments]) - for (segment, lr) in tqdm(p1, desc="Getting Logs", total=len(segments)): - lreaders[segment] = lr + download_segments = [seg for car, seg in segments if car in tested_cars] + log_data: Dict[str, LogReader] = {} + p1 = pool.map(get_log_data, download_segments) + for segment, lr in tqdm(p1, desc="Getting Logs", total=len(download_segments)): + log_data[segment] = lr pool_args: Any = [] for car_brand, segment in segments: - if (len(args.whitelist_cars) and car_brand.upper() not in args.whitelist_cars) or \ - (not len(args.whitelist_cars) and car_brand.upper() in args.blacklist_cars): + if car_brand not in tested_cars: continue for cfg in CONFIGS: - if (len(args.whitelist_procs) and cfg.proc_name not in args.whitelist_procs) or \ - (not len(args.whitelist_procs) and cfg.proc_name in args.blacklist_procs): + if cfg.proc_name not in tested_procs: continue cur_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}_{cur_commit}.bz2") @@ -203,8 +209,8 @@ if __name__ == "__main__": ref_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}_{ref_commit}.bz2") ref_log_path = ref_log_fn if os.path.exists(ref_log_fn) else BASE_URL + os.path.basename(ref_log_fn) - lr = None if args.upload_only else lreaders[segment] - pool_args.append((segment, cfg, args, cur_log_fn, ref_log_path, lr)) + dat = None if args.upload_only else log_data[segment] + pool_args.append((segment, cfg, args, cur_log_fn, ref_log_path, dat)) results: Any = defaultdict(dict) p2 = pool.map(run_test_process, pool_args) diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 725b0dd1ea..6cf53a046e 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -23,11 +23,11 @@ from tools.lib.logreader import LogReader PROCS = { "selfdrive.controls.controlsd": 35.0, "./loggerd": 10.0, - "./encoderd": 37.3, + "./encoderd": 12.5, "./camerad": 16.5, "./locationd": 9.1, "selfdrive.controls.plannerd": 11.7, - "./_ui": 21.0, + "./_ui": 26.4, "selfdrive.locationd.paramsd": 9.0, "./_sensord": 6.17, "selfdrive.controls.radard": 4.5, diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 7c88dd5790..4df1e072b8 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -17,7 +17,7 @@ from common.filter_simple import FirstOrderFilter from common.params import Params from common.realtime import DT_TRML, sec_since_boot from selfdrive.controls.lib.alertmanager import set_offroad_alert -from selfdrive.hardware import HARDWARE, TICI +from selfdrive.hardware import HARDWARE, TICI, AGNOS from selfdrive.loggerd.config import get_available_percent from selfdrive.statsd import statlog from selfdrive.swaglog import cloudlog @@ -31,7 +31,7 @@ NetworkStrength = log.DeviceState.NetworkStrength CURRENT_TAU = 15. # 15s time constant TEMP_TAU = 5. # 5s time constant DISCONNECT_TIMEOUT = 5. # wait 5 seconds before going offroad after disconnect so you get an alert -PANDA_STATES_TIMEOUT = int(1000 * 2.5 * DT_TRML) # 2.5x the expected pandaState frequency +PANDA_STATES_TIMEOUT = int(1000 * 1.5 * DT_TRML) # 1.5x the expected pandaState frequency ThermalBand = namedtuple("ThermalBand", ['min_temp', 'max_temp']) HardwareState = namedtuple("HardwareState", ['network_type', 'network_metered', 'network_strength', 'network_info', 'nvme_temps', 'modem_temps']) @@ -113,7 +113,7 @@ def hw_state_thread(end_event, hw_queue): modem_temps = prev_hw_state.modem_temps # Log modem version once - if TICI and ((modem_version is None) or (modem_nv is None)): + if AGNOS and ((modem_version is None) or (modem_nv is None)): modem_version = HARDWARE.get_modem_version() # pylint: disable=assignment-from-none modem_nv = HARDWARE.get_modem_nv() # pylint: disable=assignment-from-none @@ -134,7 +134,7 @@ def hw_state_thread(end_event, hw_queue): except queue.Full: pass - if TICI and (hw_state.network_info is not None) and (hw_state.network_info.get('state', None) == "REGISTERED"): + if AGNOS and (hw_state.network_info is not None) and (hw_state.network_info.get('state', None) == "REGISTERED"): registered_count += 1 else: registered_count = 0 @@ -220,6 +220,11 @@ def thermald_thread(end_event, hw_queue): if TICI: fan_controller = TiciFanController() + elif (sec_since_boot() - sm.rcv_time['pandaStates']/1e9) > DISCONNECT_TIMEOUT: + if onroad_conditions["ignition"]: + onroad_conditions["ignition"] = False + cloudlog.error("panda timed out onroad") + try: last_hw_state = hw_queue.get_nowait() except queue.Empty: @@ -244,7 +249,7 @@ def thermald_thread(end_event, hw_queue): current_filter.update(msg.deviceState.batteryCurrent / 1e6) max_comp_temp = temp_filter.update( - max(max(msg.deviceState.cpuTempC), msg.deviceState.memoryTempC, max(msg.deviceState.gpuTempC)) + max(max(msg.deviceState.cpuTempC), msg.deviceState.memoryTempC, max(msg.deviceState.gpuTempC), max(msg.deviceState.pmicTempC)) ) if fan_controller is not None: diff --git a/selfdrive/timezoned.py b/selfdrive/timezoned.py index 6a7e9122c1..fc1ca92cdf 100755 --- a/selfdrive/timezoned.py +++ b/selfdrive/timezoned.py @@ -9,7 +9,7 @@ import requests from timezonefinder import TimezoneFinder from common.params import Params -from selfdrive.hardware import TICI +from selfdrive.hardware import AGNOS from selfdrive.swaglog import cloudlog @@ -20,7 +20,7 @@ def set_timezone(valid_timezones, timezone): cloudlog.debug(f"Setting timezone to {timezone}") try: - if TICI: + if AGNOS: tzpath = os.path.join("/usr/share/zoneinfo/", timezone) subprocess.check_call(f'sudo su -c "ln -snf {tzpath} /data/etc/tmptime && \ mv /data/etc/tmptime /data/etc/localtime"', shell=True) diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index dcf3ee0f67..1d0f8532db 100755 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -10,15 +10,12 @@ import glob from typing import NoReturn from common.file_helpers import mkdirs_exists_ok -from selfdrive.hardware import TICI from selfdrive.loggerd.config import ROOT import selfdrive.sentry as sentry from selfdrive.swaglog import cloudlog from selfdrive.version import get_commit -MAX_SIZE = 100000 * 10 # mal size is 40-100k, allow up to 1M -if TICI: - MAX_SIZE = MAX_SIZE * 100 # Allow larger size for tici since files contain coredump +MAX_SIZE = 1_000_000 * 100 # allow up to 100M MAX_TOMBSTONE_FN_LEN = 62 # 85 - 23 ("/crash/") TOMBSTONE_DIR = "/data/tombstones/" diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 1ea13d5564..47c4ac2a51 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -1,8 +1,8 @@ import os -Import('qt_env', 'arch', 'common', 'messaging', 'gpucommon', 'visionipc', +Import('qt_env', 'arch', 'common', 'messaging', 'visionipc', 'cereal', 'transformations') -base_libs = [gpucommon, common, messaging, cereal, visionipc, transformations, 'zmq', +base_libs = [common, messaging, cereal, visionipc, transformations, 'zmq', 'capnp', 'kj', 'm', 'OpenCL', 'ssl', 'crypto', 'pthread'] + qt_env["LIBS"] maps = arch in ['larch64', 'x86_64'] @@ -118,11 +118,3 @@ if arch in ['x86_64', 'Darwin'] or GetOption('extras'): if GetOption('test'): qt_env.Program('replay/tests/test_replay', ['replay/tests/test_runner.cc', 'replay/tests/test_replay.cc'], LIBS=[replay_libs]) - -# navd -if maps: - navd_src = ["navd/main.cc", "navd/route_engine.cc", "navd/map_renderer.cc"] - qt_env.Program("navd/_navd", navd_src, LIBS=qt_libs + ['common', 'json11']) - - if GetOption('extras'): - qt_env.SharedLibrary("navd/map_renderer", ["navd/map_renderer.cc"], LIBS=qt_libs + ['common', 'messaging']) diff --git a/selfdrive/ui/navd/.gitignore b/selfdrive/ui/navd/.gitignore deleted file mode 100644 index 0fa7b173e4..0000000000 --- a/selfdrive/ui/navd/.gitignore +++ /dev/null @@ -1 +0,0 @@ -_navd diff --git a/selfdrive/ui/navd/main.cc b/selfdrive/ui/navd/main.cc deleted file mode 100644 index fe354b7b7d..0000000000 --- a/selfdrive/ui/navd/main.cc +++ /dev/null @@ -1,45 +0,0 @@ -#include -#include -#include -#include -#include - -#include "selfdrive/ui/qt/util.h" -#include "selfdrive/ui/qt/maps/map_helpers.h" -#include "selfdrive/ui/navd/route_engine.h" -#include "selfdrive/ui/navd/map_renderer.h" -#include "selfdrive/hardware/hw.h" -#include "common/params.h" - -void sigHandler(int s) { - qInfo() << "Shutting down"; - std::signal(s, SIG_DFL); - - qApp->quit(); -} - - -int main(int argc, char *argv[]) { - qInstallMessageHandler(swagLogMessageHandler); - - QApplication app(argc, argv); - std::signal(SIGINT, sigHandler); - std::signal(SIGTERM, sigHandler); - - QCommandLineParser parser; - parser.setApplicationDescription("Navigation server. Runs stand-alone, or using pre-computer route"); - parser.addHelpOption(); - parser.process(app); - const QStringList args = parser.positionalArguments(); - - - RouteEngine* route_engine = new RouteEngine(); - - if (Params().getBool("NavdRender")) { - MapRenderer * m = new MapRenderer(get_mapbox_settings()); - QObject::connect(route_engine, &RouteEngine::positionUpdated, m, &MapRenderer::updatePosition); - QObject::connect(route_engine, &RouteEngine::routeUpdated, m, &MapRenderer::updateRoute); - } - - return app.exec(); -} diff --git a/selfdrive/ui/navd/map_renderer.cc b/selfdrive/ui/navd/map_renderer.cc deleted file mode 100644 index 8d2a8810c9..0000000000 --- a/selfdrive/ui/navd/map_renderer.cc +++ /dev/null @@ -1,200 +0,0 @@ -#include "selfdrive/ui/navd/map_renderer.h" - -#include -#include -#include - -#include "selfdrive/ui/qt/maps/map_helpers.h" -#include "common/timing.h" - -const float ZOOM = 13.5; // Don't go below 13 or features will start to disappear -const int WIDTH = 256; -const int HEIGHT = WIDTH; - -const int NUM_VIPC_BUFFERS = 4; - -MapRenderer::MapRenderer(const QMapboxGLSettings &settings, bool enable_vipc) : m_settings(settings) { - QSurfaceFormat fmt; - fmt.setRenderableType(QSurfaceFormat::OpenGLES); - - ctx = std::make_unique(); - ctx->setFormat(fmt); - ctx->create(); - assert(ctx->isValid()); - - surface = std::make_unique(); - surface->setFormat(ctx->format()); - surface->create(); - - ctx->makeCurrent(surface.get()); - assert(QOpenGLContext::currentContext() == ctx.get()); - - gl_functions.reset(ctx->functions()); - gl_functions->initializeOpenGLFunctions(); - - QOpenGLFramebufferObjectFormat fbo_format; - fbo.reset(new QOpenGLFramebufferObject(WIDTH, HEIGHT, fbo_format)); - - m_map.reset(new QMapboxGL(nullptr, m_settings, fbo->size(), 1)); - m_map->setCoordinateZoom(QMapbox::Coordinate(0, 0), ZOOM); - m_map->setStyleUrl("mapbox://styles/commaai/ckvmksrpd4n0a14pfdo5heqzr"); - m_map->createRenderer(); - - m_map->resize(fbo->size()); - m_map->setFramebufferObject(fbo->handle(), fbo->size()); - gl_functions->glViewport(0, 0, WIDTH, HEIGHT); - - if (enable_vipc) { - qWarning() << "Enabling navd map rendering"; - vipc_server.reset(new VisionIpcServer("navd")); - vipc_server->create_buffers(VisionStreamType::VISION_STREAM_RGB_MAP, NUM_VIPC_BUFFERS, true, WIDTH, HEIGHT); - vipc_server->start_listener(); - - pm.reset(new PubMaster({"navThumbnail"})); - } -} - -void MapRenderer::updatePosition(QMapbox::Coordinate position, float bearing) { - if (m_map.isNull()) { - return; - } - - m_map->setCoordinate(position); - m_map->setBearing(bearing); - update(); -} - -bool MapRenderer::loaded() { - return m_map->isFullyLoaded(); -} - -void MapRenderer::update() { - gl_functions->glClear(GL_COLOR_BUFFER_BIT); - m_map->render(); - gl_functions->glFlush(); - - sendVipc(); -} - -void MapRenderer::sendVipc() { - if (!vipc_server || !loaded()) { - return; - } - - QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor); - uint64_t ts = nanos_since_boot(); - VisionBuf* buf = vipc_server->get_buffer(VisionStreamType::VISION_STREAM_RGB_MAP); - VisionIpcBufExtra extra = { - .frame_id = frame_id, - .timestamp_sof = ts, - .timestamp_eof = ts, - }; - - assert(cap.sizeInBytes() == buf->len); - memcpy(buf->addr, cap.bits(), buf->len); - vipc_server->send(buf, &extra); - - if (frame_id % 100 == 0) { - // Write jpeg into buffer - QByteArray buffer_bytes; - QBuffer buffer(&buffer_bytes); - buffer.open(QIODevice::WriteOnly); - cap.save(&buffer, "JPG", 50); - - kj::Array buffer_kj = kj::heapArray((const capnp::byte*)buffer_bytes.constData(), buffer_bytes.size()); - - // Send thumbnail - MessageBuilder msg; - auto thumbnaild = msg.initEvent().initNavThumbnail(); - thumbnaild.setFrameId(frame_id); - thumbnaild.setTimestampEof(ts); - thumbnaild.setThumbnail(buffer_kj); - pm->send("navThumbnail", msg); - } - - frame_id++; -} - -uint8_t* MapRenderer::getImage() { - QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor); - uint8_t* buf = new uint8_t[cap.sizeInBytes()]; - memcpy(buf, cap.bits(), cap.sizeInBytes()); - - return buf; -} - -void MapRenderer::updateRoute(QList coordinates) { - if (m_map.isNull()) return; - initLayers(); - - auto route_points = coordinate_list_to_collection(coordinates); - QMapbox::Feature feature(QMapbox::Feature::LineStringType, route_points, {}, {}); - QVariantMap navSource; - navSource["type"] = "geojson"; - navSource["data"] = QVariant::fromValue(feature); - m_map->updateSource("navSource", navSource); - m_map->setLayoutProperty("navLayer", "visibility", "visible"); -} - -void MapRenderer::initLayers() { - if (!m_map->layerExists("navLayer")) { - QVariantMap nav; - nav["id"] = "navLayer"; - nav["type"] = "line"; - nav["source"] = "navSource"; - m_map->addLayer(nav, "road-intersection"); - m_map->setPaintProperty("navLayer", "line-color", QColor("blue")); - m_map->setPaintProperty("navLayer", "line-width", 3); - m_map->setLayoutProperty("navLayer", "line-cap", "round"); - } -} - -MapRenderer::~MapRenderer() { -} - -extern "C" { - MapRenderer* map_renderer_init() { - char *argv[] = { - (char*)"navd", - nullptr - }; - int argc = 0; - QApplication *app = new QApplication(argc, argv); - assert(app); - - QMapboxGLSettings settings; - settings.setApiBaseUrl(MAPS_HOST); - settings.setAccessToken(get_mapbox_token()); - - return new MapRenderer(settings, false); - } - - void map_renderer_update_position(MapRenderer *inst, float lat, float lon, float bearing) { - inst->updatePosition({lat, lon}, bearing); - QApplication::processEvents(); - } - - void map_renderer_update_route(MapRenderer *inst, char* polyline) { - inst->updateRoute(polyline_to_coordinate_list(QString::fromUtf8(polyline))); - } - - void map_renderer_update(MapRenderer *inst) { - inst->update(); - } - - void map_renderer_process(MapRenderer *inst) { - QApplication::processEvents(); - } - - bool map_renderer_loaded(MapRenderer *inst) { - return inst->loaded(); - } - - uint8_t * map_renderer_get_image(MapRenderer *inst) { - return inst->getImage(); - } - - void map_renderer_free_image(MapRenderer *inst, uint8_t * buf) { - delete[] buf; - } -} diff --git a/selfdrive/ui/navd/map_renderer.h b/selfdrive/ui/navd/map_renderer.h deleted file mode 100644 index 1746e76695..0000000000 --- a/selfdrive/ui/navd/map_renderer.h +++ /dev/null @@ -1,49 +0,0 @@ -#pragma once - -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "cereal/visionipc/visionipc_server.h" -#include "cereal/messaging/messaging.h" - - -class MapRenderer : public QObject { - Q_OBJECT - -public: - MapRenderer(const QMapboxGLSettings &, bool enable_vipc=true); - uint8_t* getImage(); - void update(); - bool loaded(); - ~MapRenderer(); - - -private: - std::unique_ptr ctx; - std::unique_ptr surface; - std::unique_ptr gl_functions; - std::unique_ptr fbo; - - std::unique_ptr vipc_server; - std::unique_ptr pm; - void sendVipc(); - - QMapboxGLSettings m_settings; - QScopedPointer m_map; - - void initLayers(); - - uint32_t frame_id = 0; - -public slots: - void updatePosition(QMapbox::Coordinate position, float bearing); - void updateRoute(QList coordinates); -}; diff --git a/selfdrive/ui/navd/map_renderer.py b/selfdrive/ui/navd/map_renderer.py deleted file mode 100755 index 28d006841b..0000000000 --- a/selfdrive/ui/navd/map_renderer.py +++ /dev/null @@ -1,78 +0,0 @@ -#!/usr/bin/env python3 -# You might need to uninstall the PyQt5 pip package to avoid conflicts - -import os -import time -from cffi import FFI - -from common.ffi_wrapper import suffix -from common.basedir import BASEDIR - -HEIGHT = WIDTH = 256 - - -def get_ffi(): - lib = os.path.join(BASEDIR, "selfdrive", "ui", "navd", "libmap_renderer" + suffix()) - - ffi = FFI() - ffi.cdef(""" -void* map_renderer_init(); -void map_renderer_update_position(void *inst, float lat, float lon, float bearing); -void map_renderer_update_route(void *inst, char *polyline); -void map_renderer_update(void *inst); -void map_renderer_process(void *inst); -bool map_renderer_loaded(void *inst); -uint8_t* map_renderer_get_image(void *inst); -void map_renderer_free_image(void *inst, uint8_t *buf); -""") - return ffi, ffi.dlopen(lib) - - -def wait_ready(lib, renderer): - while not lib.map_renderer_loaded(renderer): - lib.map_renderer_update(renderer) - - # The main qt app is not execed, so we need to periodically process events for e.g. network requests - lib.map_renderer_process(renderer) - - time.sleep(0.01) - - -def get_image(lib, renderer): - buf = lib.map_renderer_get_image(renderer) - r = list(buf[0:3 * WIDTH * HEIGHT]) - lib.map_renderer_free_image(renderer, buf) - - # Convert to numpy - r = np.asarray(r) - return r.reshape((WIDTH, HEIGHT, 3)) - - -if __name__ == "__main__": - import matplotlib.pyplot as plt - import numpy as np - - ffi, lib = get_ffi() - renderer = lib.map_renderer_init() - wait_ready(lib, renderer) - - geometry = r"{yxk}@|obn~Eg@@eCFqc@J{RFw@?kA@gA?q|@Riu@NuJBgi@ZqVNcRBaPBkG@iSD{I@_H@cH?gG@mG@gG?aD@{LDgDDkVVyQLiGDgX@q_@@qI@qKhS{R~[}NtYaDbGoIvLwNfP_b@|f@oFnF_JxHel@bf@{JlIuxAlpAkNnLmZrWqFhFoh@jd@kX|TkJxH_RnPy^|[uKtHoZ~Um`DlkCorC``CuShQogCtwB_ThQcr@fk@sVrWgRhVmSb\\oj@jxA{Qvg@u]tbAyHzSos@xjBeKbWszAbgEc~@~jCuTrl@cYfo@mRn\\_m@v}@ij@jp@om@lk@y|A`pAiXbVmWzUod@xj@wNlTw}@|uAwSn\\kRfYqOdS_IdJuK`KmKvJoOhLuLbHaMzGwO~GoOzFiSrEsOhD}PhCqw@vJmnAxSczA`Vyb@bHk[fFgl@pJeoDdl@}}@zIyr@hG}X`BmUdBcM^aRR}Oe@iZc@mR_@{FScHxAn_@vz@zCzH~GjPxAhDlB~DhEdJlIbMhFfG|F~GlHrGjNjItLnGvQ~EhLnBfOn@p`@AzAAvn@CfC?fc@`@lUrArStCfSxEtSzGxM|ElFlBrOzJlEbDnC~BfDtCnHjHlLvMdTnZzHpObOf^pKla@~G|a@dErg@rCbj@zArYlj@ttJ~AfZh@r]LzYg@`TkDbj@gIdv@oE|i@kKzhA{CdNsEfOiGlPsEvMiDpLgBpHyB`MkB|MmArPg@|N?|P^rUvFz~AWpOCdAkB|PuB`KeFfHkCfGy@tAqC~AsBPkDs@uAiAcJwMe@s@eKkPMoXQux@EuuCoH?eI?Kas@}Dy@wAUkMOgDL" - lib.map_renderer_update_route(renderer, geometry.encode()) - - POSITIONS = [ - (32.71569271952601, -117.16384270868463, 0), (32.71569271952601, -117.16384270868463, 45), # San Diego - (52.378641991483136, 4.902623379456488, 0), (52.378641991483136, 4.902623379456488, 45), # Amsterdam - ] - plt.figure() - - for i, pos in enumerate(POSITIONS): - t = time.time() - lib.map_renderer_update_position(renderer, *pos) - wait_ready(lib, renderer) - - print(f"{pos} took {time.time() - t:.2f} s") - - plt.subplot(2, 2, i + 1) - plt.imshow(get_image(lib, renderer)) - - plt.show() diff --git a/selfdrive/ui/navd/navd b/selfdrive/ui/navd/navd deleted file mode 100755 index ff3103bd97..0000000000 --- a/selfdrive/ui/navd/navd +++ /dev/null @@ -1,4 +0,0 @@ -#!/bin/sh -cd "$(dirname "$0")" -export QT_PLUGIN_PATH="../../../third_party/qt-plugins/$(uname -m)" -exec ./_navd diff --git a/selfdrive/ui/navd/route_engine.cc b/selfdrive/ui/navd/route_engine.cc deleted file mode 100644 index 577f267c9b..0000000000 --- a/selfdrive/ui/navd/route_engine.cc +++ /dev/null @@ -1,359 +0,0 @@ -#include "selfdrive/ui/navd/route_engine.h" - -#include - -#include "selfdrive/ui/qt/maps/map.h" -#include "selfdrive/ui/qt/maps/map_helpers.h" -#include "selfdrive/ui/qt/api.h" - -#include "common/params.h" - -const qreal REROUTE_DISTANCE = 25; -const float MANEUVER_TRANSITION_THRESHOLD = 10; - -static float get_time_typical(const QGeoRouteSegment &segment) { - auto maneuver = segment.maneuver(); - auto attrs = maneuver.extendedAttributes(); - return attrs.contains("mapbox.duration_typical") ? attrs["mapbox.duration_typical"].toDouble() : segment.travelTime(); -} - -static cereal::NavInstruction::Direction string_to_direction(QString d) { - if (d.contains("left")) { - return cereal::NavInstruction::Direction::LEFT; - } else if (d.contains("right")) { - return cereal::NavInstruction::Direction::RIGHT; - } else if (d.contains("straight")) { - return cereal::NavInstruction::Direction::STRAIGHT; - } - - return cereal::NavInstruction::Direction::NONE; -} - -static void parse_banner(cereal::NavInstruction::Builder &instruction, const QMap &banner, bool full) { - QString primary_str, secondary_str; - - auto p = banner["primary"].toMap(); - primary_str += p["text"].toString(); - - instruction.setShowFull(full); - - if (p.contains("type")) { - instruction.setManeuverType(p["type"].toString().toStdString()); - } - - if (p.contains("modifier")) { - instruction.setManeuverModifier(p["modifier"].toString().toStdString()); - } - - if (banner.contains("secondary")) { - auto s = banner["secondary"].toMap(); - secondary_str += s["text"].toString(); - } - - instruction.setManeuverPrimaryText(primary_str.toStdString()); - instruction.setManeuverSecondaryText(secondary_str.toStdString()); - - if (banner.contains("sub")) { - auto s = banner["sub"].toMap(); - auto components = s["components"].toList(); - - size_t num_lanes = 0; - for (auto &c : components) { - auto cc = c.toMap(); - if (cc["type"].toString() == "lane") { - num_lanes += 1; - } - } - - auto lanes = instruction.initLanes(num_lanes); - - size_t i = 0; - for (auto &c : components) { - auto cc = c.toMap(); - if (cc["type"].toString() == "lane") { - auto lane = lanes[i]; - lane.setActive(cc["active"].toBool()); - - if (cc.contains("active_direction")) { - lane.setActiveDirection(string_to_direction(cc["active_direction"].toString())); - } - - auto directions = lane.initDirections(cc["directions"].toList().size()); - - size_t j = 0; - for (auto &dir : cc["directions"].toList()) { - directions.set(j, string_to_direction(dir.toString())); - j++; - } - - - i++; - } - } - } -} - -RouteEngine::RouteEngine() { - sm = new SubMaster({"liveLocationKalman", "managerState"}); - pm = new PubMaster({"navInstruction", "navRoute"}); - - // Timers - route_timer = new QTimer(this); - QObject::connect(route_timer, SIGNAL(timeout()), this, SLOT(routeUpdate())); - route_timer->start(1000); - - msg_timer = new QTimer(this); - QObject::connect(msg_timer, SIGNAL(timeout()), this, SLOT(msgUpdate())); - msg_timer->start(50); - - // Build routing engine - QVariantMap parameters; - parameters["mapbox.access_token"] = get_mapbox_token(); - parameters["mapbox.directions_api_url"] = MAPS_HOST + "/directions/v5/mapbox/"; - - geoservice_provider = new QGeoServiceProvider("mapbox", parameters); - routing_manager = geoservice_provider->routingManager(); - if (routing_manager == nullptr) { - qWarning() << geoservice_provider->errorString(); - assert(routing_manager); - } - QObject::connect(routing_manager, &QGeoRoutingManager::finished, this, &RouteEngine::routeCalculated); - - // Get last gps position from params - auto last_gps_position = coordinate_from_param("LastGPSPosition"); - if (last_gps_position) { - last_position = *last_gps_position; - } -} - -void RouteEngine::msgUpdate() { - sm->update(1000); - if (!sm->updated("liveLocationKalman")) { - active = false; - return; - } - - - if (sm->updated("managerState")) { - for (auto const &p : (*sm)["managerState"].getManagerState().getProcesses()) { - if (p.getName() == "ui" && p.getRunning()) { - if (ui_pid && *ui_pid != p.getPid()){ - qWarning() << "UI restarting, sending route"; - QTimer::singleShot(5000, this, &RouteEngine::sendRoute); - } - ui_pid = p.getPid(); - } - } - } - - auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman(); - auto pos = location.getPositionGeodetic(); - auto orientation = location.getCalibratedOrientationNED(); - - gps_ok = location.getGpsOK(); - - localizer_valid = (location.getStatus() == cereal::LiveLocationKalman::Status::VALID) && pos.getValid(); - - if (localizer_valid) { - last_bearing = RAD2DEG(orientation.getValue()[2]); - last_position = QMapbox::Coordinate(pos.getValue()[0], pos.getValue()[1]); - emit positionUpdated(*last_position, *last_bearing); - } - - active = true; -} - -void RouteEngine::routeUpdate() { - if (!active) { - return; - } - - recomputeRoute(); - - MessageBuilder msg; - cereal::Event::Builder evt = msg.initEvent(segment.isValid()); - cereal::NavInstruction::Builder instruction = evt.initNavInstruction(); - - // Show route instructions - if (segment.isValid()) { - auto cur_maneuver = segment.maneuver(); - auto attrs = cur_maneuver.extendedAttributes(); - if (cur_maneuver.isValid() && attrs.contains("mapbox.banner_instructions")) { - float along_geometry = distance_along_geometry(segment.path(), to_QGeoCoordinate(*last_position)); - float distance_to_maneuver_along_geometry = segment.distance() - along_geometry; - - auto banners = attrs["mapbox.banner_instructions"].toList(); - if (banners.size()) { - auto banner = banners[0].toMap(); - - for (auto &b : banners) { - auto bb = b.toMap(); - if (distance_to_maneuver_along_geometry < bb["distance_along_geometry"].toDouble()) { - banner = bb; - } - } - - instruction.setManeuverDistance(distance_to_maneuver_along_geometry); - parse_banner(instruction, banner, distance_to_maneuver_along_geometry < banner["distance_along_geometry"].toDouble()); - - // ETA - float progress = distance_along_geometry(segment.path(), to_QGeoCoordinate(*last_position)) / segment.distance(); - float total_distance = segment.distance() * (1.0 - progress); - float total_time = segment.travelTime() * (1.0 - progress); - float total_time_typical = get_time_typical(segment) * (1.0 - progress); - - auto s = segment.nextRouteSegment(); - while (s.isValid()) { - total_distance += s.distance(); - total_time += s.travelTime(); - total_time_typical += get_time_typical(s); - - s = s.nextRouteSegment(); - } - instruction.setTimeRemaining(total_time); - instruction.setTimeRemainingTypical(total_time_typical); - instruction.setDistanceRemaining(total_distance); - } - - // Transition to next route segment - if (distance_to_maneuver_along_geometry < -MANEUVER_TRANSITION_THRESHOLD) { - auto next_segment = segment.nextRouteSegment(); - if (next_segment.isValid()) { - segment = next_segment; - - recompute_backoff = 0; - recompute_countdown = 0; - } else { - qWarning() << "Destination reached"; - Params().remove("NavDestination"); - - // Clear route if driving away from destination - float d = segment.maneuver().position().distanceTo(to_QGeoCoordinate(*last_position)); - if (d > REROUTE_DISTANCE) { - clearRoute(); - } - } - } - } - } - - pm->send("navInstruction", msg); -} - -void RouteEngine::clearRoute() { - route = QGeoRoute(); - segment = QGeoRouteSegment(); - nav_destination = QMapbox::Coordinate(); -} - -bool RouteEngine::shouldRecompute() { - if (!segment.isValid()) { - return true; - } - - // Don't recompute in last segment, assume destination is reached - if (!segment.nextRouteSegment().isValid()) { - return false; - } - - // Compute closest distance to all line segments in the current path - float min_d = REROUTE_DISTANCE + 1; - auto path = segment.path(); - auto cur = to_QGeoCoordinate(*last_position); - for (size_t i = 0; i < path.size() - 1; i++) { - auto a = path[i]; - auto b = path[i+1]; - if (a.distanceTo(b) < 1.0) { - continue; - } - min_d = std::min(min_d, minimum_distance(a, b, cur)); - } - return min_d > REROUTE_DISTANCE; - - // TODO: Check for going wrong way in segment -} - -void RouteEngine::recomputeRoute() { - if (!last_position) { - return; - } - - auto new_destination = coordinate_from_param("NavDestination"); - if (!new_destination) { - clearRoute(); - return; - } - - bool should_recompute = shouldRecompute(); - if (*new_destination != nav_destination) { - qWarning() << "Got new destination from NavDestination param" << *new_destination; - should_recompute = true; - } - - if (!gps_ok && segment.isValid()) return; // Don't recompute when gps drifts in tunnels - - if (recompute_countdown == 0 && should_recompute) { - recompute_countdown = std::pow(2, recompute_backoff); - recompute_backoff = std::min(6, recompute_backoff + 1); - calculateRoute(*new_destination); - } else { - recompute_countdown = std::max(0, recompute_countdown - 1); - } -} - -void RouteEngine::calculateRoute(QMapbox::Coordinate destination) { - qWarning() << "Calculating route" << *last_position << "->" << destination; - - nav_destination = destination; - QGeoRouteRequest request(to_QGeoCoordinate(*last_position), to_QGeoCoordinate(destination)); - request.setFeatureWeight(QGeoRouteRequest::TrafficFeature, QGeoRouteRequest::AvoidFeatureWeight); - - if (last_bearing) { - QVariantMap params; - int bearing = ((int)(*last_bearing) + 360) % 360; - params["bearing"] = bearing; - request.setWaypointsMetadata({params}); - } - - routing_manager->calculateRoute(request); -} - -void RouteEngine::routeCalculated(QGeoRouteReply *reply) { - if (reply->error() == QGeoRouteReply::NoError) { - if (reply->routes().size() != 0) { - qWarning() << "Got route response"; - - route = reply->routes().at(0); - segment = route.firstRouteSegment(); - - auto path = route.path(); - emit routeUpdated(path); - } else { - qWarning() << "Got empty route response"; - } - } else { - qWarning() << "Got error in route reply" << reply->errorString(); - } - - sendRoute(); - - reply->deleteLater(); -} - -void RouteEngine::sendRoute() { - MessageBuilder msg; - cereal::Event::Builder evt = msg.initEvent(); - cereal::NavRoute::Builder nav_route = evt.initNavRoute(); - - auto path = route.path(); - auto coordinates = nav_route.initCoordinates(path.size()); - - size_t i = 0; - for (auto const &c : route.path()) { - coordinates[i].setLatitude(c.latitude()); - coordinates[i].setLongitude(c.longitude()); - i++; - } - - pm->send("navRoute", msg); -} diff --git a/selfdrive/ui/navd/route_engine.h b/selfdrive/ui/navd/route_engine.h deleted file mode 100644 index 33cbc79107..0000000000 --- a/selfdrive/ui/navd/route_engine.h +++ /dev/null @@ -1,62 +0,0 @@ -#pragma once - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "cereal/messaging/messaging.h" - -class RouteEngine : public QObject { - Q_OBJECT - -public: - RouteEngine(); - - SubMaster *sm; - PubMaster *pm; - - QTimer* msg_timer; - QTimer* route_timer; - - std::optional ui_pid; - - // Route - bool gps_ok = false; - QGeoServiceProvider *geoservice_provider; - QGeoRoutingManager *routing_manager; - QGeoRoute route; - QGeoRouteSegment segment; - QMapbox::Coordinate nav_destination; - - // Position - std::optional last_position; - std::optional last_bearing; - bool localizer_valid = false; - - // Route recompute - bool active = false; - int recompute_backoff = 0; - int recompute_countdown = 0; - void calculateRoute(QMapbox::Coordinate destination); - void clearRoute(); - bool shouldRecompute(); - -private slots: - void routeUpdate(); - void msgUpdate(); - void routeCalculated(QGeoRouteReply *reply); - void recomputeRoute(); - void sendRoute(); - -signals: - void positionUpdated(QMapbox::Coordinate position, float bearing); - void routeUpdated(QList coordinates); -}; diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index 8e5a02f8a0..4c6a0a4e65 100644 --- a/selfdrive/ui/qt/maps/map.cc +++ b/selfdrive/ui/qt/maps/map.cc @@ -3,14 +3,14 @@ #include #include -#include #include +#include #include "common/swaglog.h" -#include "selfdrive/ui/ui.h" -#include "selfdrive/ui/qt/util.h" #include "selfdrive/ui/qt/maps/map_helpers.h" #include "selfdrive/ui/qt/request_repeater.h" +#include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/ui.h" const int PAN_TIMEOUT = 100; @@ -24,16 +24,8 @@ const float MAP_SCALE = 2; const QString ICON_SUFFIX = ".png"; -MapWindow::MapWindow(const QMapboxGLSettings &settings) : - m_settings(settings), velocity_filter(0, 10, 0.05) { - sm = new SubMaster({"liveLocationKalman", "navInstruction", "navRoute"}); - - // Connect now, so any navRoutes sent while the map is initializing are not dropped - sm->update(0); - - timer = new QTimer(this); - QObject::connect(timer, SIGNAL(timeout()), this, SLOT(timerUpdate())); - timer->start(50); +MapWindow::MapWindow(const QMapboxGLSettings &settings) : m_settings(settings), velocity_filter(0, 10, 0.05) { + QObject::connect(uiState(), &UIState::uiUpdate, this, &MapWindow::updateState); // Instructions map_instructions = new MapInstructions(this); @@ -105,16 +97,15 @@ void MapWindow::initLayers() { } } -void MapWindow::timerUpdate() { +void MapWindow::updateState(const UIState &s) { if (!uiState()->scene.started) { return; } - + const SubMaster &sm = *(s.sm); update(); - sm->update(0); - if (sm->updated("liveLocationKalman")) { - auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman(); + if (sm.updated("liveLocationKalman")) { + auto location = sm["liveLocationKalman"].getLiveLocationKalman(); auto pos = location.getPositionGeodetic(); auto orientation = location.getCalibratedOrientationNED(); auto velocity = location.getVelocityCalibrated(); @@ -129,7 +120,7 @@ void MapWindow::timerUpdate() { } } - if (sm->updated("navRoute") && (*sm)["navRoute"].getNavRoute().getCoordinates().size()) { + if (sm.updated("navRoute") && sm["navRoute"].getNavRoute().getCoordinates().size()) { qWarning() << "Got new navRoute from navd. Opening map:" << allow_open; // Only open the map on setting destination the first time @@ -178,9 +169,9 @@ void MapWindow::timerUpdate() { zoom_counter--; } - if (sm->updated("navInstruction")) { - if (sm->valid("navInstruction")) { - auto i = (*sm)["navInstruction"].getNavInstruction(); + if (sm.updated("navInstruction")) { + if (sm.valid("navInstruction")) { + auto i = sm["navInstruction"].getNavInstruction(); emit ETAChanged(i.getTimeRemaining(), i.getTimeRemainingTypical(), i.getDistanceRemaining()); if (localizer_valid) { @@ -194,9 +185,9 @@ void MapWindow::timerUpdate() { } } - if (sm->rcv_frame("navRoute") != route_rcv_frame) { + if (sm.rcv_frame("navRoute") != route_rcv_frame) { qWarning() << "Updating navLayer with new route"; - auto route = (*sm)["navRoute"].getNavRoute(); + auto route = sm["navRoute"].getNavRoute(); auto route_points = capnp_coordinate_list_to_collection(route.getCoordinates()); QMapbox::Feature feature(QMapbox::Feature::LineStringType, route_points, {}, {}); QVariantMap navSource; @@ -205,7 +196,7 @@ void MapWindow::timerUpdate() { m_map->updateSource("navSource", navSource); m_map->setLayoutProperty("navLayer", "visibility", "visible"); - route_rcv_frame = sm->rcv_frame("navRoute"); + route_rcv_frame = sm.rcv_frame("navRoute"); } } diff --git a/selfdrive/ui/qt/maps/map.h b/selfdrive/ui/qt/maps/map.h index 01a13f3b7c..7c39b24c3c 100644 --- a/selfdrive/ui/qt/maps/map.h +++ b/selfdrive/ui/qt/maps/map.h @@ -5,22 +5,22 @@ #include #include #include -#include #include +#include #include #include #include +#include #include #include -#include -#include +#include #include -#include -#include +#include +#include "cereal/messaging/messaging.h" #include "common/params.h" #include "common/util.h" -#include "cereal/messaging/messaging.h" +#include "selfdrive/ui/ui.h" class MapInstructions : public QWidget { Q_OBJECT @@ -91,8 +91,6 @@ private: void pinchTriggered(QPinchGesture *gesture); bool m_sourceAdded = false; - SubMaster *sm; - QTimer* timer; bool loaded_once = false; bool allow_open = true; @@ -115,7 +113,7 @@ private: uint64_t route_rcv_frame = 0; private slots: - void timerUpdate(); + void updateState(const UIState &s); public slots: void offroadTransition(bool offroad); diff --git a/selfdrive/ui/qt/maps/map_helpers.cc b/selfdrive/ui/qt/maps/map_helpers.cc index be1098994b..83576eb630 100644 --- a/selfdrive/ui/qt/maps/map_helpers.cc +++ b/selfdrive/ui/qt/maps/map_helpers.cc @@ -101,101 +101,6 @@ QMapbox::CoordinatesCollections coordinate_list_to_collection(QList polyline_to_coordinate_list(const QString &polylineString) { - QList path; - if (polylineString.isEmpty()) - return path; - - QByteArray data = polylineString.toLatin1(); - - bool parsingLatitude = true; - - int shift = 0; - int value = 0; - - QGeoCoordinate coord(0, 0); - - for (int i = 0; i < data.length(); ++i) { - unsigned char c = data.at(i) - 63; - - value |= (c & 0x1f) << shift; - shift += 5; - - // another chunk - if (c & 0x20) - continue; - - int diff = (value & 1) ? ~(value >> 1) : (value >> 1); - - if (parsingLatitude) { - coord.setLatitude(coord.latitude() + (double)diff/1e6); - } else { - coord.setLongitude(coord.longitude() + (double)diff/1e6); - path.append(coord); - } - - parsingLatitude = !parsingLatitude; - - value = 0; - shift = 0; - } - - return path; -} - -static QGeoCoordinate sub(QGeoCoordinate v, QGeoCoordinate w) { - return QGeoCoordinate(v.latitude() - w.latitude(), v.longitude() - w.longitude()); -} - -static QGeoCoordinate add(QGeoCoordinate v, QGeoCoordinate w) { - return QGeoCoordinate(v.latitude() + w.latitude(), v.longitude() + w.longitude()); -} - -static QGeoCoordinate mul(QGeoCoordinate v, float c) { - return QGeoCoordinate(c * v.latitude(), c * v.longitude()); -} - -static float dot(QGeoCoordinate v, QGeoCoordinate w) { - return v.latitude() * w.latitude() + v.longitude() * w.longitude(); -} - -float minimum_distance(QGeoCoordinate a, QGeoCoordinate b, QGeoCoordinate p) { - // If a and b are the same coordinate the computation below doesn't work - if (a.distanceTo(b) < 0.01) { - return a.distanceTo(p); - } - - const QGeoCoordinate ap = sub(p, a); - const QGeoCoordinate ab = sub(b, a); - const float t = std::clamp(dot(ap, ab) / dot(ab, ab), 0.0f, 1.0f); - const QGeoCoordinate projection = add(a, mul(ab, t)); - return projection.distanceTo(p); -} - -float distance_along_geometry(QList geometry, QGeoCoordinate pos) { - if (geometry.size() <= 2) { - return geometry[0].distanceTo(pos); - } - - // 1. Find segment that is closest to current position - // 2. Total distance is sum of distance to start of closest segment - // + all previous segments - double total_distance = 0; - double total_distance_closest = 0; - double closest_distance = std::numeric_limits::max(); - - for (int i = 0; i < geometry.size() - 1; i++) { - double d = minimum_distance(geometry[i], geometry[i+1], pos); - if (d < closest_distance) { - closest_distance = d; - total_distance_closest = total_distance + geometry[i].distanceTo(pos); - } - total_distance += geometry[i].distanceTo(geometry[i+1]); - } - - return total_distance_closest; -} - std::optional coordinate_from_param(std::string param) { QString json_str = QString::fromStdString(Params().get(param)); if (json_str.isEmpty()) return {}; diff --git a/selfdrive/ui/qt/maps/map_helpers.h b/selfdrive/ui/qt/maps/map_helpers.h index cda4cd1cfb..344246bb05 100644 --- a/selfdrive/ui/qt/maps/map_helpers.h +++ b/selfdrive/ui/qt/maps/map_helpers.h @@ -24,8 +24,5 @@ QMapbox::CoordinatesCollections model_to_collection( QMapbox::CoordinatesCollections coordinate_to_collection(QMapbox::Coordinate c); QMapbox::CoordinatesCollections capnp_coordinate_list_to_collection(const capnp::List::Reader &coordinate_list); QMapbox::CoordinatesCollections coordinate_list_to_collection(QList coordinate_list); -QList polyline_to_coordinate_list(const QString &polylineString); -float minimum_distance(QGeoCoordinate a, QGeoCoordinate b, QGeoCoordinate p); std::optional coordinate_from_param(std::string param); -float distance_along_geometry(QList geometry, QGeoCoordinate pos); diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 00e120d867..4aff797714 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -162,7 +162,7 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) { power_layout->addWidget(poweroff_btn); QObject::connect(poweroff_btn, &QPushButton::clicked, this, &DevicePanel::poweroff); - if (Hardware::TICI()) { + if (!Hardware::PC()) { connect(uiState(), &UIState::offroadTransition, poweroff_btn, &QPushButton::setVisible); } diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index bedb570235..31d94c2c65 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -91,7 +91,7 @@ void OnroadWindow::offroadTransition(bool offroad) { alerts->updateAlert({}, bg); // update stream type - bool wide_cam = Hardware::TICI() && Params().getBool("EnableWideCamera"); + bool wide_cam = Params().getBool("WideCameraOnly"); nvg->setStreamType(wide_cam ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD); } diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index a68280773a..0a3b9762e5 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -34,17 +34,15 @@ const char frame_fragment_shader[] = "precision mediump float;\n" #endif "uniform sampler2D uTextureY;\n" - "uniform sampler2D uTextureU;\n" - "uniform sampler2D uTextureV;\n" + "uniform sampler2D uTextureUV;\n" "in vec2 vTexCoord;\n" "out vec4 colorOut;\n" "void main() {\n" " float y = texture(uTextureY, vTexCoord).r;\n" - " float u = texture(uTextureU, vTexCoord).r - 0.5;\n" - " float v = texture(uTextureV, vTexCoord).r - 0.5;\n" - " float r = y + 1.402 * v;\n" - " float g = y - 0.344 * u - 0.714 * v;\n" - " float b = y + 1.772 * u;\n" + " vec2 uv = texture(uTextureUV, vTexCoord).rg - 0.5;\n" + " float r = y + 1.402 * uv.y;\n" + " float g = y - 0.344 * uv.x - 0.714 * uv.y;\n" + " float b = y + 1.772 * uv.x;\n" " colorOut = vec4(r, g, b, 1.0);\n" "}\n"; @@ -57,10 +55,9 @@ const mat4 device_transform = {{ mat4 get_driver_view_transform(int screen_width, int screen_height, int stream_width, int stream_height) { const float driver_view_ratio = 2.0; - mat4 transform; const float yscale = stream_height * driver_view_ratio / stream_width; const float xscale = yscale*screen_height/screen_width*stream_width/stream_height; - transform = (mat4){{ + mat4 transform = (mat4){{ xscale, 0.0, 0.0, 0.0, 0.0, yscale, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, @@ -148,8 +145,7 @@ void CameraViewWidget::initializeGL() { glGenTextures(3, textures); glUseProgram(program->programId()); glUniform1i(program->uniformLocation("uTextureY"), 0); - glUniform1i(program->uniformLocation("uTextureU"), 1); - glUniform1i(program->uniformLocation("uTextureV"), 2); + glUniform1i(program->uniformLocation("uTextureUV"), 1); } void CameraViewWidget::showEvent(QShowEvent *event) { @@ -213,19 +209,23 @@ void CameraViewWidget::paintGL() { VisionBuf *frame = frames[frame_idx].second; glPixelStorei(GL_UNPACK_ALIGNMENT, 1); + glPixelStorei(GL_UNPACK_ROW_LENGTH, stream_stride); glViewport(0, 0, width(), height()); glBindVertexArray(frame_vao); glUseProgram(program->programId()); - uint8_t *address[3] = {frame->y, frame->u, frame->v}; - for (int i = 0; i < 3; ++i) { - glActiveTexture(GL_TEXTURE0 + i); - glBindTexture(GL_TEXTURE_2D, textures[i]); - int width = i == 0 ? stream_width : stream_width / 2; - int height = i == 0 ? stream_height : stream_height / 2; - glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, width, height, GL_RED, GL_UNSIGNED_BYTE, address[i]); - assert(glGetError() == GL_NO_ERROR); - } + + glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_2D, textures[0]); + glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, stream_width, stream_height, GL_RED, GL_UNSIGNED_BYTE, frame->y); + assert(glGetError() == GL_NO_ERROR); + + glPixelStorei(GL_UNPACK_ROW_LENGTH, stream_stride/2); + + glActiveTexture(GL_TEXTURE0 + 1); + glBindTexture(GL_TEXTURE_2D, textures[1]); + glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, stream_width/2, stream_height/2, GL_RG, GL_UNSIGNED_BYTE, frame->uv); + assert(glGetError() == GL_NO_ERROR); glUniformMatrix4fv(program->uniformLocation("uTransform"), 1, GL_TRUE, frame_mat.v); assert(glGetError() == GL_NO_ERROR); @@ -236,6 +236,7 @@ void CameraViewWidget::paintGL() { glBindTexture(GL_TEXTURE_2D, 0); glActiveTexture(GL_TEXTURE0); glPixelStorei(GL_UNPACK_ALIGNMENT, 4); + glPixelStorei(GL_UNPACK_ROW_LENGTH, 0); } void CameraViewWidget::vipcConnected(VisionIpcClient *vipc_client) { @@ -243,18 +244,23 @@ void CameraViewWidget::vipcConnected(VisionIpcClient *vipc_client) { frames.clear(); stream_width = vipc_client->buffers[0].width; stream_height = vipc_client->buffers[0].height; + stream_stride = vipc_client->buffers[0].stride; + + glBindTexture(GL_TEXTURE_2D, textures[0]); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); + glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); + glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); + glTexImage2D(GL_TEXTURE_2D, 0, GL_R8, stream_width, stream_height, 0, GL_RED, GL_UNSIGNED_BYTE, nullptr); + assert(glGetError() == GL_NO_ERROR); - for (int i = 0; i < 3; ++i) { - glBindTexture(GL_TEXTURE_2D, textures[i]); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); - glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); - glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); - int width = i == 0 ? stream_width : stream_width / 2; - int height = i == 0 ? stream_height : stream_height / 2; - glTexImage2D(GL_TEXTURE_2D, 0, GL_R8, width, height, 0, GL_RED, GL_UNSIGNED_BYTE, nullptr); - assert(glGetError() == GL_NO_ERROR); - } + glBindTexture(GL_TEXTURE_2D, textures[1]); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); + glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); + glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RG8, stream_width/2, stream_height/2, 0, GL_RG, GL_UNSIGNED_BYTE, nullptr); + assert(glGetError() == GL_NO_ERROR); updateFrameMat(width(), height()); } diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h index 65d5edc221..953cbed00b 100644 --- a/selfdrive/ui/qt/widgets/cameraview.h +++ b/selfdrive/ui/qt/widgets/cameraview.h @@ -49,6 +49,7 @@ protected: std::string stream_name; int stream_width = 0; int stream_height = 0; + int stream_stride = 0; std::atomic stream_type; QThread *vipc_thread = nullptr; diff --git a/selfdrive/ui/replay/framereader.cc b/selfdrive/ui/replay/framereader.cc index ca839e6819..8453407ce7 100644 --- a/selfdrive/ui/replay/framereader.cc +++ b/selfdrive/ui/replay/framereader.cc @@ -229,18 +229,21 @@ AVFrame *FrameReader::decodeFrame(AVPacket *pkt) { bool FrameReader::copyBuffers(AVFrame *f, uint8_t *yuv) { if (hw_pix_fmt == HW_PIX_FMT) { uint8_t *y = yuv ? yuv : nv12toyuv_buffer.data(); - uint8_t *u = y + width * height; - uint8_t *v = u + (width / 2) * (height / 2); - libyuv::NV12ToI420(f->data[0], f->linesize[0], f->data[1], f->linesize[1], - y, width, u, width / 2, v, width / 2, width, height); + uint8_t *uv = y + width * height; + for (int i = 0; i < height/2; i++) { + memcpy(y + (i*2 + 0)*width, f->data[0] + (i*2 + 0)*f->linesize[0], width); + memcpy(y + (i*2 + 1)*width, f->data[0] + (i*2 + 1)*f->linesize[0], width); + memcpy(uv + i*width, f->data[1] + i*f->linesize[1], width); + } } else { - uint8_t *u = yuv + width * height; - uint8_t *v = u + (width / 2) * (height / 2); - libyuv::I420Copy(f->data[0], f->linesize[0], - f->data[1], f->linesize[1], - f->data[2], f->linesize[2], - yuv, width, u, width / 2, v, width / 2, - width, height); + uint8_t *y = yuv ? yuv : nv12toyuv_buffer.data(); + uint8_t *uv = y + width * height; + libyuv::I420ToNV12(f->data[0], f->linesize[0], + f->data[1], f->linesize[1], + f->data[2], f->linesize[2], + y, width, + uv, width / 2, + width, height); } return true; } diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 16ddbab050..c9f81c0539 100755 --- a/selfdrive/ui/ui +++ b/selfdrive/ui/ui @@ -1,6 +1,5 @@ #!/bin/sh cd "$(dirname "$0")" export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH" -export QT_PLUGIN_PATH="../../third_party/qt-plugins/$(uname -m)" export QT_DBL_CLICK_DIST=150 exec ./_ui diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 985b0259d2..811438832b 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -182,7 +182,7 @@ static void update_state(UIState *s) { } } } - if (Hardware::TICI() && sm.updated("wideRoadCameraState")) { + if (sm.updated("wideRoadCameraState")) { auto camera_state = sm["wideRoadCameraState"].getWideRoadCameraState(); float max_lines = 1618; @@ -222,7 +222,7 @@ void UIState::updateStatus() { status = STATUS_DISENGAGED; scene.started_frame = sm->frame; scene.end_to_end = Params().getBool("EndToEndToggle"); - wide_camera = Hardware::TICI() ? Params().getBool("EnableWideCamera") : false; + wide_camera = Params().getBool("WideCameraOnly"); } started_prev = scene.started; emit offroadTransition(!scene.started); @@ -233,11 +233,11 @@ UIState::UIState(QObject *parent) : QObject(parent) { sm = std::make_unique>({ "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "roadCameraState", "pandaStates", "carParams", "driverMonitoringState", "sensorEvents", "carState", "liveLocationKalman", - "wideRoadCameraState", "managerState", + "wideRoadCameraState", "managerState", "navInstruction", "navRoute", }); Params params; - wide_camera = Hardware::TICI() ? params.getBool("EnableWideCamera") : false; + wide_camera = params.getBool("WideCameraOnly"); prime_type = std::atoi(params.get("PrimeType").c_str()); // update timer diff --git a/selfdrive/ui/watch3.cc b/selfdrive/ui/watch3.cc index c1d47d040d..00d23ea976 100644 --- a/selfdrive/ui/watch3.cc +++ b/selfdrive/ui/watch3.cc @@ -19,8 +19,6 @@ int main(int argc, char *argv[]) { { QHBoxLayout *hlayout = new QHBoxLayout(); layout->addLayout(hlayout); - // TODO: make mapd output YUV - // hlayout->addWidget(new CameraViewWidget("navd", VISION_STREAM_MAP, false)); hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_ROAD, false)); } diff --git a/selfdrive/updated.py b/selfdrive/updated.py index 19dba9825e..f835522cdb 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -37,7 +37,7 @@ from markdown_it import MarkdownIt from common.basedir import BASEDIR from common.params import Params -from selfdrive.hardware import TICI, HARDWARE +from selfdrive.hardware import AGNOS, HARDWARE from selfdrive.swaglog import cloudlog from selfdrive.controls.lib.alertmanager import set_offroad_alert from selfdrive.version import is_tested_branch @@ -328,7 +328,7 @@ def fetch_update(wait_helper: WaitTimeHelper) -> bool: ] cloudlog.info("git reset success: %s", '\n'.join(r)) - if TICI: + if AGNOS: handle_agnos_update(wait_helper) # Create the finalized, ready-to-swap update diff --git a/third_party/qt-plugins/build_qtlocation.sh b/third_party/qt-plugins/build_qtlocation.sh deleted file mode 100755 index 09e6182d1f..0000000000 --- a/third_party/qt-plugins/build_qtlocation.sh +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env sh - -# Qtlocation plugin with extra fields parsed from api response -cd /tmp -git clone https://github.com/commaai/qtlocation.git -cd qtlocation -qmake -make -j$(nproc) diff --git a/third_party/qt-plugins/x86_64/geoservices/.gitattributes b/third_party/qt-plugins/x86_64/geoservices/.gitattributes deleted file mode 100644 index 51b1d333fe..0000000000 --- a/third_party/qt-plugins/x86_64/geoservices/.gitattributes +++ /dev/null @@ -1 +0,0 @@ -libqtgeoservices_mapbox.so filter=lfs diff=lfs merge=lfs -text diff --git a/third_party/qt-plugins/x86_64/geoservices/libqtgeoservices_mapbox.so b/third_party/qt-plugins/x86_64/geoservices/libqtgeoservices_mapbox.so deleted file mode 100755 index 375fc0e012..0000000000 --- a/third_party/qt-plugins/x86_64/geoservices/libqtgeoservices_mapbox.so +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:5c8bda321dc524e753f67823cb5353358e756cfc1c5328e22e32920e80dd9e9b -size 12166248 diff --git a/tools/lib/framereader.py b/tools/lib/framereader.py index e333683b59..d9920ab128 100644 --- a/tools/lib/framereader.py +++ b/tools/lib/framereader.py @@ -185,20 +185,28 @@ def read_file_check_size(f, sz, cookie): return buff -def rgb24toyuv420(rgb): +def rgb24toyuv(rgb): yuv_from_rgb = np.array([[ 0.299 , 0.587 , 0.114 ], [-0.14714119, -0.28886916, 0.43601035 ], [ 0.61497538, -0.51496512, -0.10001026 ]]) img = np.dot(rgb.reshape(-1, 3), yuv_from_rgb.T).reshape(rgb.shape) - y_len = img.shape[0] * img.shape[1] - uv_len = y_len // 4 + ys = img[:, :, 0] us = (img[::2, ::2, 1] + img[1::2, ::2, 1] + img[::2, 1::2, 1] + img[1::2, 1::2, 1]) / 4 + 128 vs = (img[::2, ::2, 2] + img[1::2, ::2, 2] + img[::2, 1::2, 2] + img[1::2, 1::2, 2]) / 4 + 128 - yuv420 = np.empty(y_len + 2 * uv_len, dtype=img.dtype) + return ys, us, vs + + +def rgb24toyuv420(rgb): + ys, us, vs = rgb24toyuv(rgb) + + y_len = rgb.shape[0] * rgb.shape[1] + uv_len = y_len // 4 + + yuv420 = np.empty(y_len + 2 * uv_len, dtype=rgb.dtype) yuv420[:y_len] = ys.reshape(-1) yuv420[y_len:y_len + uv_len] = us.reshape(-1) yuv420[y_len + uv_len:y_len + 2 * uv_len] = vs.reshape(-1) @@ -206,6 +214,20 @@ def rgb24toyuv420(rgb): return yuv420.clip(0, 255).astype('uint8') +def rgb24tonv12(rgb): + ys, us, vs = rgb24toyuv(rgb) + + y_len = rgb.shape[0] * rgb.shape[1] + uv_len = y_len // 4 + + nv12 = np.empty(y_len + 2 * uv_len, dtype=rgb.dtype) + nv12[:y_len] = ys.reshape(-1) + nv12[y_len::2] = us.reshape(-1) + nv12[y_len+1::2] = vs.reshape(-1) + + return nv12.clip(0, 255).astype('uint8') + + def decompress_video_data(rawdat, vid_fmt, w, h, pix_fmt): # using a tempfile is much faster than proc.communicate for some reason @@ -237,6 +259,8 @@ def decompress_video_data(rawdat, vid_fmt, w, h, pix_fmt): if pix_fmt == "rgb24": ret = np.frombuffer(dat, dtype=np.uint8).reshape(-1, h, w, 3) + elif pix_fmt == "nv12": + ret = np.frombuffer(dat, dtype=np.uint8).reshape(-1, (h*w*3//2)) elif pix_fmt == "yuv420p": ret = np.frombuffer(dat, dtype=np.uint8).reshape(-1, (h*w*3//2)) elif pix_fmt == "yuv444p": @@ -304,7 +328,7 @@ class RawFrameReader(BaseFrameReader): assert self.frame_count is not None assert num+count <= self.frame_count - if pix_fmt not in ("yuv420p", "rgb24"): + if pix_fmt not in ("nv12", "yuv420p", "rgb24"): raise ValueError(f"Unsupported pixel format {pix_fmt!r}") app = [] @@ -313,6 +337,8 @@ class RawFrameReader(BaseFrameReader): rgb_dat = self.load_and_debayer(dat) if pix_fmt == "rgb24": app.append(rgb_dat) + elif pix_fmt == "nv12": + app.append(rgb24tonv12(rgb_dat)) elif pix_fmt == "yuv420p": app.append(rgb24toyuv420(rgb_dat)) else: @@ -329,7 +355,7 @@ class VideoStreamDecompressor: self.h = h self.pix_fmt = pix_fmt - if pix_fmt == "yuv420p": + if pix_fmt in ("nv12", "yuv420p"): self.out_size = w*h*3//2 # yuv420p elif pix_fmt in ("rgb24", "yuv444p"): self.out_size = w*h*3 @@ -387,6 +413,8 @@ class VideoStreamDecompressor: ret = np.frombuffer(dat, dtype=np.uint8).reshape((self.h, self.w, 3)) elif self.pix_fmt == "yuv420p": ret = np.frombuffer(dat, dtype=np.uint8) + elif self.pix_fmt == "nv12": + ret = np.frombuffer(dat, dtype=np.uint8) elif self.pix_fmt == "yuv444p": ret = np.frombuffer(dat, dtype=np.uint8).reshape((3, self.h, self.w)) else: @@ -549,7 +577,7 @@ class GOPFrameReader(BaseFrameReader): if num + count > self.frame_count: raise ValueError(f"{num + count} > {self.frame_count}") - if pix_fmt not in ("yuv420p", "rgb24", "yuv444p"): + if pix_fmt not in ("nv12", "yuv420p", "rgb24", "yuv444p"): raise ValueError(f"Unsupported pixel format {pix_fmt!r}") ret = [self._get_one(num + i, pix_fmt) for i in range(count)] diff --git a/tools/lib/logreader.py b/tools/lib/logreader.py index b4d3de67fb..46c648ef12 100755 --- a/tools/lib/logreader.py +++ b/tools/lib/logreader.py @@ -74,22 +74,24 @@ class MultiLogIterator: class LogReader: - def __init__(self, fn, canonicalize=True, only_union_types=False, sort_by_time=False): + def __init__(self, fn, canonicalize=True, only_union_types=False, sort_by_time=False, dat=None): self.data_version = None self._only_union_types = only_union_types - _, ext = os.path.splitext(urllib.parse.urlparse(fn).path) - with FileReader(fn) as f: - dat = f.read() + ext = None + if not dat: + _, ext = os.path.splitext(urllib.parse.urlparse(fn).path) + if ext not in ('', '.bz2'): + # old rlogs weren't bz2 compressed + raise Exception(f"unknown extension {ext}") - if ext == "": - # old rlogs weren't bz2 compressed - ents = capnp_log.Event.read_multiple_bytes(dat) - elif ext == ".bz2": + with FileReader(fn) as f: + dat = f.read() + + if ext == ".bz2" or dat.startswith(b'BZh9'): dat = bz2.decompress(dat) - ents = capnp_log.Event.read_multiple_bytes(dat) - else: - raise Exception(f"unknown extension {ext}") + + ents = capnp_log.Event.read_multiple_bytes(dat) _ents = [] try: @@ -101,6 +103,10 @@ class LogReader: self._ents = list(sorted(_ents, key=lambda x: x.logMonoTime) if sort_by_time else _ents) self._ts = [x.logMonoTime for x in self._ents] + @classmethod + def from_bytes(cls, dat): + return cls("", dat=dat) + def __iter__(self): for ent in self._ents: if self._only_union_types: @@ -112,7 +118,6 @@ class LogReader: else: yield ent - def logreader_from_route_or_segment(r, sort_by_time=False): sn = SegmentName(r, allow_route_name=True) route = Route(sn.route_name.canonical_name) diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py index dbf8f578fd..ae65e8deb8 100755 --- a/tools/sim/bridge.py +++ b/tools/sim/bridge.py @@ -36,6 +36,7 @@ def parse_args(add_args=None): parser = argparse.ArgumentParser(description='Bridge between CARLA and openpilot.') parser.add_argument('--joystick', action='store_true') parser.add_argument('--high_quality', action='store_true') + parser.add_argument('--dual_camera', action='store_true') parser.add_argument('--town', type=str, default='Town04_Opt') parser.add_argument('--spawn_point', dest='num_selected_spawn_point', type=int, default=16) @@ -112,7 +113,7 @@ class Camerad: dat = messaging.new_message(pub_type) msg = { - "frameId": image.frame, + "frameId": frame_id, "transform": [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] @@ -239,6 +240,7 @@ class CarlaBridge: msg.liveCalibration.validBlocks = 20 msg.liveCalibration.rpyCalib = [0.0, 0.0, 0.0] Params().put("CalibrationParams", msg.to_bytes()) + Params().put_bool("WideCameraOnly", not arguments.dual_camera) self._args = arguments self._carla_objects = [] @@ -333,10 +335,13 @@ class CarlaBridge: return camera self._camerad = Camerad() - road_camera = create_camera(fov=40, callback=self._camerad.cam_callback_road) - road_wide_camera = create_camera(fov=120, callback=self._camerad.cam_callback_wide_road) # fov bigger than 120 shows unwanted artifacts - self._carla_objects.extend([road_camera, road_wide_camera]) + if self._args.dual_camera: + road_camera = create_camera(fov=40, callback=self._camerad.cam_callback_road) + self._carla_objects.append(road_camera) + + road_wide_camera = create_camera(fov=120, callback=self._camerad.cam_callback_wide_road) # fov bigger than 120 shows unwanted artifacts + self._carla_objects.append(road_wide_camera) vehicle_state = VehicleState() @@ -504,6 +509,7 @@ class CarlaBridge: def close(self): self.started = False self._exit_event.set() + for s in self._carla_objects: try: s.destroy() @@ -522,17 +528,23 @@ if __name__ == "__main__": q: Any = Queue() args = parse_args() - carla_bridge = CarlaBridge(args) - p = carla_bridge.run(q) + try: + carla_bridge = CarlaBridge(args) + p = carla_bridge.run(q) - if args.joystick: - # start input poll for joystick - from tools.sim.lib.manual_ctrl import wheel_poll_thread + if args.joystick: + # start input poll for joystick + from tools.sim.lib.manual_ctrl import wheel_poll_thread - wheel_poll_thread(q) - else: - # start input poll for keyboard - from tools.sim.lib.keyboard_ctrl import keyboard_poll_thread + wheel_poll_thread(q) + else: + # start input poll for keyboard + from tools.sim.lib.keyboard_ctrl import keyboard_poll_thread + + keyboard_poll_thread(q) + p.join() - keyboard_poll_thread(q) - p.join() + finally: + # Try cleaning up the wide camera param + # in case users want to use replay after + Params().delete("WideCameraOnly") diff --git a/tools/sim/launch_openpilot.sh b/tools/sim/launch_openpilot.sh index f5bac8a864..82fc4a71ac 100755 --- a/tools/sim/launch_openpilot.sh +++ b/tools/sim/launch_openpilot.sh @@ -5,7 +5,7 @@ export NOBOARD="1" export SIMULATION="1" export FINGERPRINT="HONDA CIVIC 2016" -export BLOCK="camerad,loggerd" +export BLOCK="camerad,loggerd,encoderd" DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" cd ../../selfdrive/manager && exec ./manager.py