From f9aa22bfada238fee28a8353e5e0d618daf769ee Mon Sep 17 00:00:00 2001 From: BogGyver Date: Sun, 12 Apr 2020 01:20:14 -0400 Subject: [PATCH] identify camera by id not by video channel; make ui respond to window resolution; add parameters for RoadUsbCameraID and DriverUSBCameraID; --- common/params.py | 2 + selfdrive/camerad/cameras/camera_webcam.cc | 18 +++++--- selfdrive/car/tesla/carstate.py | 2 + selfdrive/car/tesla/readconfig.py | 35 ++++++++++++++ selfdrive/car/tesla/readconfig.sh | 3 +- selfdrive/ui/SConscript | 2 +- selfdrive/ui/bbui.h | 33 +++++++++++++ selfdrive/ui/bbuistate.h | 9 ++++ selfdrive/ui/linux.cc | 14 +++--- selfdrive/ui/paint.cc | 30 +++++++++--- selfdrive/ui/ui | 2 +- selfdrive/ui/ui.cc | 54 +++++++++++++++++++++- selfdrive/ui/ui.hpp | 7 ++- 13 files changed, 186 insertions(+), 25 deletions(-) diff --git a/common/params.py b/common/params.py index ac2ada7aecda7c..f2df25d7804e01 100755 --- a/common/params.py +++ b/common/params.py @@ -107,6 +107,8 @@ class UnknownKeyName(Exception): "Offroad_PandaFirmwareMismatch": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], "Offroad_InvalidTime": [TxType.CLEAR_ON_MANAGER_START], "Offroad_IsTakingSnapshot": [TxType.CLEAR_ON_MANAGER_START], + "DriverUsbCameraID": [TxType.PERSISTENT], + "RoadUsbCameraID": [TxType.PERSISTENT], } diff --git a/selfdrive/camerad/cameras/camera_webcam.cc b/selfdrive/camerad/cameras/camera_webcam.cc index 54b73a08e3ab5e..1efda839339623 100644 --- a/selfdrive/camerad/cameras/camera_webcam.cc +++ b/selfdrive/camerad/cameras/camera_webcam.cc @@ -14,6 +14,7 @@ #include #include #include +#include "common/params.h" extern volatile sig_atomic_t do_exit; #define FRAME_WIDTH 1164 @@ -37,16 +38,16 @@ void camera_release_buffer(void *cookie, int buf_idx) { } -void open_gl_stream_def(CameraState * s, int video_id, int width, int height, char ** strm_def) { +void open_gl_stream_def(CameraState * s, char* camera_id, int width, int height, char ** strm_def) { printf("OPENGLSTREAM"); - std::string strm_template="v4l2src device=/dev/video%d ! video/x-raw,width=%d,height=%d,framerate=%d/1,format=YUY2 !" + std::string strm_template="v4l2src device=/dev/v4l/by-id/%s ! video/x-raw,width=%d,height=%d,framerate=%d/1,format=YUY2 !" " nvvidconv ! video/x-raw(memory:NVMM),format=I420 !" " nvvidconv ! video/x-raw,format=BGRx !" " videoconvert ! video/x-raw,format=BGR !" " videoscale ! video/x-raw,width=%d,height=%d !" " appsink "; - * strm_def = (char*)calloc(300,1); - sprintf(*strm_def,strm_template.c_str(),video_id, width, height, s->fps, s->ci.frame_width, s->ci.frame_height); + * strm_def = (char*)calloc(600,1); + sprintf(*strm_def,strm_template.c_str(),camera_id, width, height, s->fps, s->ci.frame_width, s->ci.frame_height); printf(" GL Stream :[%s]\n",*strm_def); } @@ -67,7 +68,9 @@ static void* rear_thread(void *arg) { CameraState* s = (CameraState*)arg; char * strm_def; printf("open_GL"); - open_gl_stream_def(s,1, 800, 600, &strm_def); + char * cameraId_value; + const int result = read_db_value(NULL, "RoadUsbCameraID", &cameraId_value, NULL); + open_gl_stream_def(s,cameraId_value, 800, 600, &strm_def); cv::VideoCapture cap_rear(strm_def); // road free(strm_def); @@ -122,7 +125,10 @@ void front_thread(CameraState *s) { int err; printf("OPEN FRONT"); char * strm_def; - open_gl_stream_def(s,0,640,480, &strm_def); + char * cameraId_value; + const int result = read_db_value(NULL, "DriverUsbCameraID", &cameraId_value, NULL); + if (result != 0) return; + open_gl_stream_def(s,cameraId_value,640,480, &strm_def); cv::VideoCapture cap_front(strm_def); // driver free(strm_def); diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index 99233b2936d06f..a3fdb63962d9d0 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -233,6 +233,8 @@ def __init__(self, CP): self.ldwNumbPeriod = 1.5 self.tapBlinkerExtension = 2 self.ahbOffDuration = 5 + self.roadCameraID = "" + self.driverCameraID = "" #read config file read_config_file(self) ### END OF MAIN CONFIG OPTIONS ### diff --git a/selfdrive/car/tesla/readconfig.py b/selfdrive/car/tesla/readconfig.py index bca0413931894d..f288aab3ea5992 100644 --- a/selfdrive/car/tesla/readconfig.py +++ b/selfdrive/car/tesla/readconfig.py @@ -1,4 +1,6 @@ import configparser +from common.params import Params +import subprocess default_config_file_path = '/data/bb_openpilot.cfg' @@ -10,6 +12,7 @@ class ConfigFile(): def read(self, into, config_path): configr = configparser.RawConfigParser() file_changed = False + params = Params() try: configr.read(config_path) @@ -22,10 +25,12 @@ def read(self, into, config_path): main_section = 'OP_CONFIG' pref_section = 'OP_PREFERENCES' + jetson_section = 'JETSON_PREFERENCES' logging_section = 'LOGGING' config = configparser.RawConfigParser(allow_no_value=True) config.add_section(main_section) config.add_section(pref_section) + config.add_section(jetson_section) config.add_section(logging_section) #user_handle -> userHandle @@ -217,6 +222,7 @@ def read(self, into, config_path): comment = 'If you use an aftermarket Tesla Bosch Radar that already has a coded VIN, you will have to enter that VIN value here.' ) file_changed |= didUpdate + if into.radarVIN == '': into.radarVIN = default_radar_vin file_changed = True @@ -338,6 +344,33 @@ def read(self, into, config_path): ) file_changed |= didUpdate + #jetson_road_camera_id -> roadCameraID + into.roadCameraID, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = jetson_section, + entry = 'jetson_road_camera_id', entry_type = str, + default_value = 'NotSet', + comment = 'ID of camera facing road, as seen in ls -al /dev/v4l/by-id' + ) + file_changed |= didUpdate + + #jetson_driver_camera_id -> driverCameraID + into.driverCameraID, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = jetson_section, + entry = 'jetson_driver_camera_id', entry_type = str, + default_value = 'NotSet', + comment = 'ID of camera facing driver, as seen in ls -al /dev/v4l/by-id' + ) + file_changed |= didUpdate + + #check camera_id values against LiveParams + savedRoadCameraID = params.get("RoadUsbCameraID") + savedDriverCameraID = params.get("DriverUsbCameraID") + if into.driverCameraID != savedDriverCameraID: + params.put("DriverUsbCameraID",into.driverCameraID) + if into.roadCameraID != savedRoadCameraID: + params.put("RoadUsbCameraID",into.roadCameraID) + + into.shouldLogCanErrors, didUpdate = self.read_config_entry( config, configr, prev_file_contents, section = logging_section, entry = 'should_log_can_errors', entry_type = bool, @@ -427,6 +460,8 @@ class CarSettings(): tapBlinkerExtension = None ahbOffDuration = None usesApillarHarness = None + roadCameraID = None + driverCameraID = None def __init__(self, optional_config_file_path = default_config_file_path): config_file = ConfigFile() diff --git a/selfdrive/car/tesla/readconfig.sh b/selfdrive/car/tesla/readconfig.sh index 352ab9e66720f5..3c3184d09dbcd1 100755 --- a/selfdrive/car/tesla/readconfig.sh +++ b/selfdrive/car/tesla/readconfig.sh @@ -1,4 +1,3 @@ -#!/usr/bin/bash CFG_FILE=/data/bb_openpilot.cfg CFG_CONTENT=$(cat $CFG_FILE | sed -r '/[^=]+=[^=]+/!d' | sed -r 's/\s+=\s/=/g') -eval "$CFG_CONTENT" +eval "export $CFG_CONTENT" diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 4084cf0958158b..5f2773437daf22 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -13,7 +13,7 @@ if larch == "aarch64": linkflags = ['-Wl,-rpath=/system/lib64,-rpath=/system/comma/usr/lib'] else: src += ['linux.cc'] - libs += ['pthread', 'glfw'] + libs += ['pthread', 'glfw', 'X11'] linkflags = [] env.Program('_ui', src, diff --git a/selfdrive/ui/bbui.h b/selfdrive/ui/bbui.h index 4d0368f4a85d22..66f9b5fcf7fa8e 100644 --- a/selfdrive/ui/bbui.h +++ b/selfdrive/ui/bbui.h @@ -2,6 +2,19 @@ #include "cereal/gen/c/ui.capnp.h" +#if !defined(QCOM) && !defined(QCOM2) +#ifndef __APPLE__ +#define GLFW_INCLUDE_ES2 +#else +#define GLFW_INCLUDE_GLCOREARB +#endif + +#define GLFW_INCLUDE_GLEXT +#include +int linux_abs_x = 0; +int linux_abs_y = 0; +UIState *mouse_ui_state; +#endif // TODO: this is also hardcoded in common/transformations/camera.py @@ -251,7 +264,13 @@ int bb_ui_draw_measure( UIState *s, const char* bb_value, const char* bb_uom, c } + bool bb_handle_ui_touch( UIState *s, int touch_x, int touch_y) { +#if !defined(QCOM) && !defined(QCOM2) + touch_x = (int)(vwp_w * touch_x / 1280); + touch_y = (int)(vwp_h * touch_y / 720); + printf("Linux mouse up at %d, %d ( %d, %d)\n",(int)touch_x, (int)touch_y, linux_abs_x, linux_abs_y); +#endif for(int i=0; i<6; i++) { if (s->b.btns_r[i] > 0) { if ((abs(touch_x - s->b.btns_x[i]) < s->b.btns_r[i]) && (abs(touch_y - s->b.btns_y[i]) < s->b.btns_r[i])) { @@ -292,6 +311,20 @@ bool bb_handle_ui_touch( UIState *s, int touch_x, int touch_y) { return false; }; +#if !defined(QCOM) && !defined(QCOM2) +void bb_mouse_event_handler(GLFWwindow* window, int button, int action, int mods) { + if (button == GLFW_MOUSE_BUTTON_LEFT && action == GLFW_RELEASE) { + + double xpos, ypos; + glfwGetCursorPos(window, &xpos, &ypos); + int w_width,w_height; + glfwGetWindowSize(window, &linux_abs_x, &linux_abs_y); + bb_handle_ui_touch(mouse_ui_state,(int) xpos, (int)ypos); + } +} +#endif + + int bb_get_button_status( UIState *s, char *btn_name) { int ret_status = -1; for (int i = 0; i< 6; i++) { diff --git a/selfdrive/ui/bbuistate.h b/selfdrive/ui/bbuistate.h index 6b328a22a4dcc7..38df2f16e461e7 100644 --- a/selfdrive/ui/bbuistate.h +++ b/selfdrive/ui/bbuistate.h @@ -8,6 +8,15 @@ typedef struct UICstmButton { } UICstmButton; typedef struct BBUIState { + float scr_scale_x; + float scr_scale_y; + int scr_w; + int scr_h; + float scr_device_factor; + float scr_scissor_offset; +#if !defined(QCOM) && !defined(QCOM2) + Display *scr_display; +#endif int touch_last_x; int touch_last_y; bool touch_last; diff --git a/selfdrive/ui/linux.cc b/selfdrive/ui/linux.cc index 8e4a8492dcc208..b8016cec1788c1 100644 --- a/selfdrive/ui/linux.cc +++ b/selfdrive/ui/linux.cc @@ -13,16 +13,14 @@ #endif #define GLFW_INCLUDE_GLEXT -#include +#include "GLFW/glfw3.h" typedef struct FramebufferState FramebufferState; typedef struct TouchState TouchState; -extern "C" { - -FramebufferState* framebuffer_init( +FramebufferState* framebuffer_init_linux( const char* name, int32_t layer, int alpha, - int *out_w, int *out_h) { + int *out_w, int *out_h, GLFWmousebuttonfun mouse_event_handler) { glfwInit(); #ifndef __APPLE__ @@ -41,7 +39,9 @@ FramebufferState* framebuffer_init( if (!window) { printf("glfwCreateWindow failed\n"); } - + if (mouse_event_handler != NULL) { + glfwSetMouseButtonCallback(window,mouse_event_handler); + } glfwMakeContextCurrent(window); glfwSwapInterval(0); @@ -55,6 +55,8 @@ FramebufferState* framebuffer_init( return (FramebufferState*)window; } +extern "C" { + void framebuffer_set_power(FramebufferState *s, int mode) { } diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 999e36eea630fc..255bb968b10c94 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -808,8 +808,13 @@ static void ui_draw_vision(UIState *s) { // Draw video frames glEnable(GL_SCISSOR_TEST); - glViewport(ui_viz_rx+ui_viz_ro, s->fb_h-(box_y+box_h), viz_w, box_h); +#if defined(QCOM) || defined(QCOM2) + glViewport(ui_viz_rx+ui_viz_ro, s->fb_h-(box_y+box_h), viz_w , box_h); glScissor(ui_viz_rx, s->fb_h-(box_y+box_h), ui_viz_rw, box_h); +#else + glViewport(0, s->b.scr_h + s->b.scr_scissor_offset, s->b.scr_w, s->b.scr_w * 0.751); + glScissor(0, s->b.scr_h + s->b.scr_scissor_offset, s->b.scr_w, s->b.scr_w * 0.751); +#endif glEnable(GL_BLEND); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); draw_frame(s); @@ -819,6 +824,7 @@ static void ui_draw_vision(UIState *s) { glClear(GL_STENCIL_BUFFER_BIT); nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f); + nvgScale(s->vg,s->b.scr_scale_x,s->b.scr_scale_y); nvgSave(s->vg); // Draw augmented elements @@ -858,6 +864,8 @@ void ui_draw(UIState *s) { ui_draw_sidebar(s); ui_draw_vision(s); } else { + nvgScale(s->vg,s->b.scr_scale_x,s->b.scr_scale_y); + ui_draw_blank(s); if (!s->scene.uilayout_sidebarcollapsed) { ui_draw_sidebar(s); @@ -918,8 +926,8 @@ static const char frame_fragment_shader[] = #endif static const mat4 device_transform = {{ - 1.0, 0.0, 0.0, 0.0, - 0.0, 1.0, 0.0, 0.0, + 0.68, 0.0, 0.0, 0.0, + 0.0, 0.68, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, }}; @@ -1040,9 +1048,19 @@ void ui_nvg_init(UIState *s) { glBindBuffer(GL_ARRAY_BUFFER,0); glBindVertexArray(0); } - - s->front_frame_mat = matmul(device_transform, full_to_wide_frame_transform); - s->rear_frame_mat = matmul(device_transform, frame_transform); + #if defined(QCOM) || defined(QCOM2) + s->rear_frame_mat = matmul(device_transform, frame_transform); + s->front_frame_mat = matmul(device_transform, full_to_wide_frame_transform); + #else + mat4 device_transform_bb = {{ + s->b.scr_device_factor, 0.0, 0.0, 0.0, + 0.0, s->b.scr_device_factor, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 1.0, + }}; + s->rear_frame_mat = matmul(device_transform_bb, frame_transform); + s->front_frame_mat = matmul(device_transform_bb, full_to_wide_frame_transform); + #endif for(int i = 0;i < UI_BUF_COUNT; i++) { s->khr[i] = NULL; diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 7ea68031f21013..62080708df58fc 100755 --- a/selfdrive/ui/ui +++ b/selfdrive/ui/ui @@ -2,7 +2,7 @@ export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH" TBP=/data/tinkla_buddy_pro if test -f "$TBP"; then - exec /usr/bin/startx ./_ui + exec /usr/bin/startx ./_ui -rez 800 480 else exec ./_ui fi diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index c4ab5e9674981a..f177bb59aee84e 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -17,6 +17,13 @@ #include "ui.hpp" #include "sound.hpp" +#if !defined(QCOM) && !defined(QCOM2) +#include +int force_rez = 0; +int force_rez_w = 0; +int force_rez_h = 0; +#endif + #include "bbui.h" static int last_brightness = -1; @@ -190,14 +197,43 @@ static void ui_init(UIState *s) { s->ipc_fd = -1; // init display +#if defined(QCOM) || defined(QCOM2) s->fb = framebuffer_init("ui", 0x00010000, true, &s->fb_w, &s->fb_h); +#else + s->fb = framebuffer_init_linux("ui", 0x00010000, true, &s->fb_w, &s->fb_h,bb_mouse_event_handler); +#endif assert(s->fb); set_awake(s, true); s->model_changed = false; s->livempc_or_radarstate_changed = false; +#if defined(QCOM) || defined(QCOM2) + s->b.scr_w = vwp_w; + s->b.scr_h = vwp_h; + s->b.scr_scale_x = 1.0f; + s->b.scr_scale_y = 1.0f; + s->b.scr_device_factor = 1.0f; + s->b.scr_scissor_offset = 0.0f; +#else + //scale + if (force_rez == 1) { + s->b.scr_w = force_rez_w; + s->b.scr_h = force_rez_h; + printf("Forcing rezolution to %d x %d \n",force_rez_w,force_rez_h); + } else { + s->b.scr_display = XOpenDisplay(NULL); + Screen* xorg_s = DefaultScreenOfDisplay(s->b.scr_display); + s->b.scr_w = xorg_s->width; + s->b.scr_h = xorg_s->height; + } + s->b.scr_scale_x = (float)(s->b.scr_w) / (float)(vwp_w); + s->b.scr_scale_y = (float)(s->b.scr_h) / (float)(vwp_h); + s->b.scr_device_factor = (float)(1164) / (float)(s->b.scr_w); + s->b.scr_scissor_offset = (float)(s->b.scr_w * 3) / 4.0f - (float)(s->b.scr_h); + mouse_ui_state = s; +#endif ui_nvg_init(s); } @@ -838,8 +874,11 @@ static void* light_sensor_thread(void *args) { static void* bg_thread(void* args) { UIState *s = (UIState*)args; set_thread_name("bg"); - +#if defined(QCOM) || defined(QCOM2) FramebufferState *bg_fb = framebuffer_init("bg", 0x00001000, false, NULL, NULL); +#else + FramebufferState *bg_fb = framebuffer_init_linux("bg", 0x00001000, false, NULL, NULL, NULL); +#endif assert(bg_fb); int bg_status = -1; @@ -893,13 +932,22 @@ int main(int argc, char* argv[]) { zsys_handler_set(NULL); signal(SIGINT, (sighandler_t)set_do_exit); +#if !defined(QCOM) && !defined(QCOM2) + if (argc == 4) { + if (strcmp(argv[1],"-rez")==0) { + force_rez = 1; + force_rez_w = atoi(argv[2]); + force_rez_h = atoi(argv[3]); + } + } +#endif UIState uistate; UIState *s = &uistate; ui_init(s); //BB init our UI bb_ui_init(s); - + pthread_t connect_thread_handle; err = pthread_create(&connect_thread_handle, NULL, vision_connect_thread, s); @@ -989,6 +1037,7 @@ int main(int argc, char* argv[]) { s->b.touch_last_x = 0; s->b.touch_last_y = 0; } + //s->b.touch_last_width = s->scene.ui_viz_rw; //BB Update our cereal polls @@ -1034,6 +1083,7 @@ int main(int argc, char* argv[]) { if (s->awake) { ui_draw(s); if (s->vision_connected) { + nvgScale(s->vg,s->b.scr_scale_x,s->b.scr_scale_y); bb_ui_draw_UI(s) ; ui_draw_infobar(s); } diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index bf43476bebc33c..4154663d6f503a 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -303,8 +303,13 @@ void ui_draw_sidebar(UIState *s); void ui_nvg_init(UIState *s); static void set_awake(UIState *s, bool awake); +#if !defined(QCOM) && !defined(QCOM2) +#include "GLFW/glfw3.h" +FramebufferState* framebuffer_init_linux( + const char* name, int32_t layer, int alpha, + int *out_w, int *out_h, GLFWmousebuttonfun mouse_event_handler); +#endif #endif - // TODO: this is also hardcoded in common/transformations/camera.py const mat3 intrinsic_matrix = (mat3){{ 910., 0., 582.,