diff --git a/common/params.py b/common/params.py index 3777a2756..f1a7528ce 100755 --- a/common/params.py +++ b/common/params.py @@ -126,6 +126,10 @@ keys = { "DragonCharging": [TxType.PERSISTENT], "DragonDisCharging": [TxType.PERSISTENT], "DragonToyotaLaneDepartureWarning": [TxType.PERSISTENT], + "DragonUILane": [TxType.PERSISTENT], + "DragonUILead": [TxType.PERSISTENT], + "DragonUIPath": [TxType.PERSISTENT], + "DragonUIBlinker": [TxType.PERSISTENT], } diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index c5a2ce872..7b06fa39f 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -95,7 +95,7 @@ class CarController(object): pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): # dragonpilot, don't check for param too often as it's a kernel call - if frame % 100 == 0: + if frame % 500 == 0: self.dragon_enable_steering_on_signal = False if params.get("DragonEnableSteeringOnSignal") == "0" else True self.dragon_allow_gas = False if params.get("DragonAllowGas") == "0" else True self.dragon_lat_ctrl = False if params.get("DragonLatCtrl") == "0" else True diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 3c57c0b43..9482e24f2 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -128,7 +128,7 @@ class CarController(object): pcm_cancel_cmd, hud_alert, forwarding_camera, left_line, right_line, lead, left_lane_depart, right_lane_depart): # dragonpilot, don't check for param too often as it's a kernel call - if frame % 100 == 0: + if frame % 500 == 0: self.dragon_enable_steering_on_signal = False if params.get("DragonEnableSteeringOnSignal") == "0" else True self.dragon_allow_gas = False if params.get("DragonAllowGas") == "0" else True self.dragon_lat_ctrl = False if params.get("DragonLatCtrl") == "0" else True diff --git a/selfdrive/dragonpilot/dragonconf/__init__.py b/selfdrive/dragonpilot/dragonconf/__init__.py index cf7d9886c..de42b0342 100644 --- a/selfdrive/dragonpilot/dragonconf/__init__.py +++ b/selfdrive/dragonpilot/dragonconf/__init__.py @@ -46,6 +46,10 @@ default_conf = { 'DragonCharging': 70, 'DragonDisCharging': 60, 'DragonToyotaLaneDepartureWarning': '1', + 'DragonUILane': '1', + 'DragonUILead': '1', + 'DragonUIPath': '1', + 'DragonUIBlinker': '0', } deprecated_conf = { diff --git a/selfdrive/ui/Makefile b/selfdrive/ui/Makefile index c6cce40a5..c00d9df28 100644 --- a/selfdrive/ui/Makefile +++ b/selfdrive/ui/Makefile @@ -16,7 +16,7 @@ CXXFLAGS = -std=c++11 -fPIC -O2 $(WARN_FLAGS) ZMQ_LIBS = -l:libczmq.a -l:libzmq.a -lgnustl_shared CEREAL_LIBS = -l:libcapn.a -CEREAL_OBJS = ../../cereal/gen/c/log.capnp.o +CEREAL_OBJS = ../../cereal/gen/c/log.capnp.o ../../cereal/gen/c/car.capnp.o NANOVG_FLAGS = -I$(PHONELIBS)/nanovg JSON_FLAGS = -I$(PHONELIBS)/json/src diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index 7b772bfab..33db9f1cf 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -168,6 +168,11 @@ typedef struct UIScene { float angleSteersDes; float angleSteers; + // for blinker, from kegman + bool leftBlinker; + bool rightBlinker; + int blinker_blinkingrate; + } UIScene; typedef struct { @@ -215,6 +220,7 @@ typedef struct UIState { void *livempc_sock_raw; void *plus_sock_raw; void *map_data_sock_raw; + void *carstate_sock_raw; void *uilayout_sock_raw; @@ -304,6 +310,10 @@ typedef struct UIState { int dragon_enable_dashcam_timeout; int dragon_ui_volume_boost_timeout; int dragon_driving_ui_timeout; + int dragon_ui_lane_timeout; + int dragon_ui_lead_timeout; + int dragon_ui_path_timeout; + int dragon_ui_blinker_timeout; bool dragon_ui_speed; bool dragon_ui_event; @@ -314,6 +324,10 @@ typedef struct UIState { bool dragon_enable_dashcam; float dragon_ui_volume_boost; bool dragon_driving_ui; + bool dragon_ui_lane; + bool dragon_ui_lead; + bool dragon_ui_path; + bool dragon_ui_blinker; } UIState; @@ -534,6 +548,7 @@ static void ui_init(UIState *s) { s->radarstate_sock_raw = sub_sock(s->ctx, "tcp://127.0.0.1:8012"); s->livempc_sock_raw = sub_sock(s->ctx, "tcp://127.0.0.1:8035"); s->plus_sock_raw = sub_sock(s->ctx, "tcp://127.0.0.1:8037"); + s->carstate_sock_raw = sub_sock(s->ctx, "tcp://127.0.0.1:8021"); #ifdef SHOW_SPEEDLIMIT s->map_data_sock_raw = sub_sock(s->ctx, "tcp://127.0.0.1:8065"); @@ -709,6 +724,10 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs, read_param_bool(&s->dragon_enable_dashcam, "DragonEnableDashcam"); read_param_float(&s->dragon_ui_volume_boost, "DragonUIVolumeBoost"); read_param_bool(&s->dragon_driving_ui, "DragonDrivingUI"); + read_param_bool(&s->dragon_ui_lane, "DragonUILane"); + read_param_bool(&s->dragon_ui_lead, "DragonUILead"); + read_param_bool(&s->dragon_ui_path, "DragonUIPath"); + read_param_bool(&s->dragon_ui_blinker, "DragonUIBlinker"); // Set offsets so params don't get read at the same time @@ -726,6 +745,10 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs, s->dragon_enable_dashcam_timeout = 100; s->dragon_ui_volume_boost_timeout = 100; s->dragon_driving_ui_timeout = 100; + s->dragon_ui_lane_timeout = 100; + s->dragon_ui_lead_timeout = 100; + s->dragon_ui_path_timeout = 100; + s->dragon_ui_blinker_timeout = 100; } // Projects a point in car to space to the corresponding point in full frame @@ -1045,27 +1068,30 @@ static void ui_draw_vision_lanes(UIState *s) { update_all_lane_lines_data(s, scene->model.right_lane, pvd + MODEL_LANE_PATH_CNT); s->model_changed = false; } - // Draw left lane edge - ui_draw_lane( + if (s->dragon_ui_lane) { + // Draw left lane edge + ui_draw_lane( s, &scene->model.left_lane, pvd, nvgRGBAf(1.0, 1.0, 1.0, scene->model.left_lane.prob)); - // Draw right lane edge - ui_draw_lane( + // Draw right lane edge + ui_draw_lane( s, &scene->model.right_lane, pvd + MODEL_LANE_PATH_CNT, nvgRGBAf(1.0, 1.0, 1.0, scene->model.right_lane.prob)); - + } if(s->livempc_or_radarstate_changed) { update_all_track_data(s); s->livempc_or_radarstate_changed = false; } - // Draw vision path - ui_draw_track(s, false, &s->track_vertices[0]); - if (scene->engaged) { - // Draw MPC path when engaged - ui_draw_track(s, true, &s->track_vertices[1]); + if (s->dragon_ui_path) { + // Draw vision path + ui_draw_track(s, false, &s->track_vertices[0]); + if (scene->engaged) { + // Draw MPC path when engaged + ui_draw_track(s, true, &s->track_vertices[1]); + } } } @@ -1081,7 +1107,7 @@ static void ui_draw_world(UIState *s) { ui_draw_vision_lanes(s); } - if (scene->lead_status) { + if (s->dragon_ui_lead && scene->lead_status) { // Draw lead car indicator float fillAlpha = 0; float speedBuff = 10.; @@ -1284,28 +1310,57 @@ static void ui_draw_vision_speed(UIState *s) { const int viz_speed_x = ui_viz_rx+((ui_viz_rw/2)-(viz_speed_w/2)); char speed_str[32]; - nvgBeginPath(s->vg); - nvgRect(s->vg, viz_speed_x, box_y, viz_speed_w, header_h); - nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + // blinker, from kegman + if (s->dragon_ui_blinker) { + if(s->scene.leftBlinker) { + nvgBeginPath(s->vg); + nvgMoveTo(s->vg, viz_speed_x, box_y + header_h/4); + nvgLineTo(s->vg, viz_speed_x - viz_speed_w/2, box_y + header_h/4 + header_h/4); + nvgLineTo(s->vg, viz_speed_x, box_y + header_h/2 + header_h/4); + nvgClosePath(s->vg); + nvgFillColor(s->vg, nvgRGBA(23,134,68,s->scene.blinker_blinkingrate>=50?210:60)); + nvgFill(s->vg); + } - if (s->is_metric) { - snprintf(speed_str, sizeof(speed_str), "%d", (int)(speed * 3.6 + 0.5)); - } else { - snprintf(speed_str, sizeof(speed_str), "%d", (int)(speed * 2.2369363 + 0.5)); + if(s->scene.rightBlinker) { + nvgBeginPath(s->vg); + nvgMoveTo(s->vg, viz_speed_x+viz_speed_w, box_y + header_h/4); + nvgLineTo(s->vg, viz_speed_x+viz_speed_w + viz_speed_w/2, box_y + header_h/4 + header_h/4); + nvgLineTo(s->vg, viz_speed_x+viz_speed_w, box_y + header_h/2 + header_h/4); + nvgClosePath(s->vg); + nvgFillColor(s->vg, nvgRGBA(23,134,68,s->scene.blinker_blinkingrate>=50?210:60)); + nvgFill(s->vg); + } + + if(s->scene.leftBlinker || s->scene.rightBlinker) { + s->scene.blinker_blinkingrate -= 3; + if(s->scene.blinker_blinkingrate<0) s->scene.blinker_blinkingrate = 120; + } } - nvgFontFace(s->vg, "sans-bold"); - nvgFontSize(s->vg, 96*2.5); - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); - nvgText(s->vg, viz_speed_x+viz_speed_w/2, 240, speed_str, NULL); + if (s->dragon_ui_speed) { + nvgBeginPath(s->vg); + nvgRect(s->vg, viz_speed_x, box_y, viz_speed_w, header_h); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); - nvgFontFace(s->vg, "sans-regular"); - nvgFontSize(s->vg, 36*2.5); - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 200)); + if (s->is_metric) { + snprintf(speed_str, sizeof(speed_str), "%d", (int)(speed * 3.6 + 0.5)); + } else { + snprintf(speed_str, sizeof(speed_str), "%d", (int)(speed * 2.2369363 + 0.5)); + } + nvgFontFace(s->vg, "sans-bold"); + nvgFontSize(s->vg, 96*2.5); + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); + nvgText(s->vg, viz_speed_x+viz_speed_w/2, 240, speed_str, NULL); - if (s->is_metric) { - nvgText(s->vg, viz_speed_x+viz_speed_w/2, 320, "kph", NULL); - } else { - nvgText(s->vg, viz_speed_x+viz_speed_w/2, 320, "mph", NULL); + nvgFontFace(s->vg, "sans-regular"); + nvgFontSize(s->vg, 36*2.5); + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 200)); + + if (s->is_metric) { + nvgText(s->vg, viz_speed_x+viz_speed_w/2, 320, "kph", NULL); + } else { + nvgText(s->vg, viz_speed_x+viz_speed_w/2, 320, "mph", NULL); + } } } @@ -1345,11 +1400,11 @@ static void ui_draw_vision_event(UIState *s) { nvgBeginPath(s->vg); nvgCircle(s->vg, bg_wheel_x, (bg_wheel_y + (bdr_s*1.5)), bg_wheel_size); if (is_engaged) { - nvgFillColor(s->vg, nvgRGBA(23, 134, 68, 255)); + nvgFillColor(s->vg, nvgRGBA(23, 134, 68, 180)); } else if (is_warning) { - nvgFillColor(s->vg, nvgRGBA(218, 111, 37, 255)); + nvgFillColor(s->vg, nvgRGBA(218, 111, 37, 180)); } else if (is_engageable) { - nvgFillColor(s->vg, nvgRGBA(23, 51, 73, 255)); + nvgFillColor(s->vg, nvgRGBA(23, 51, 73, 180)); } nvgFill(s->vg); img_wheel_alpha = 1.0f; @@ -1436,9 +1491,7 @@ static void ui_draw_vision_header(UIState *s) { #ifdef SHOW_SPEEDLIMIT ui_draw_vision_speedlimit(s); #endif - if (s->dragon_ui_speed) { - ui_draw_vision_speed(s); - } + ui_draw_vision_speed(s); if (s->dragon_ui_event) { ui_draw_vision_event(s); } @@ -2129,6 +2182,15 @@ void handle_message(UIState *s, void *which) { struct cereal_LiveMapData datad; cereal_read_LiveMapData(&datad, eventd.liveMapData); s->scene.map_valid = datad.mapValid; + } else if (eventd.which == cereal_Event_carState) { + struct cereal_CarState datad; + cereal_read_CarState(&datad, eventd.carState); + + if(s->scene.leftBlinker!=datad.leftBlinker || s->scene.rightBlinker!=datad.rightBlinker) { + s->scene.blinker_blinkingrate = 100; + } + s->scene.leftBlinker = datad.leftBlinker; + s->scene.rightBlinker = datad.rightBlinker; } capn_free(&ctx); zmq_msg_close(&msg); @@ -2287,12 +2349,16 @@ static void ui_update(UIState *s) { polls[6].socket = s->uilayout_sock_raw; polls[6].events = ZMQ_POLLIN; -#ifdef SHOW_SPEEDLIMIT +//#ifdef SHOW_SPEEDLIMIT +// plus_sock_num++; +// num_polls++; +// polls[7].socket = s->map_data_sock_raw; +// polls[7].events = ZMQ_POLLIN; +//#endif plus_sock_num++; num_polls++; - polls[7].socket = s->map_data_sock_raw; + polls[7].socket = s->carstate_sock_raw; polls[7].events = ZMQ_POLLIN; -#endif polls[plus_sock_num].socket = s->plus_sock_raw; // plus_sock should be last polls[plus_sock_num].events = ZMQ_POLLIN; @@ -2308,6 +2374,7 @@ static void ui_update(UIState *s) { if (polls[0].revents || polls[1].revents || polls[2].revents || polls[3].revents || polls[4].revents || polls[6].revents || + polls[7].revents || polls[plus_sock_num].revents) { // awake on any (old) activity set_awake(s, true); @@ -2684,6 +2751,10 @@ int main(int argc, char* argv[]) { read_param_bool_timeout(&s->dragon_enable_dashcam, "DragonEnableDashcam", &s->dragon_enable_dashcam_timeout); read_param_float_timeout(&s->dragon_ui_volume_boost, "DragonUIVolumeBoost", &s->dragon_ui_volume_boost_timeout); read_param_bool_timeout(&s->dragon_driving_ui, "DragonDrivingUI", &s->dragon_driving_ui_timeout); + read_param_bool_timeout(&s->dragon_ui_lane, "DragonUILane", &s->dragon_ui_lane_timeout); + read_param_bool_timeout(&s->dragon_ui_lead, "DragonUILead", &s->dragon_ui_lead_timeout); + read_param_bool_timeout(&s->dragon_ui_path, "DragonUIPath", &s->dragon_ui_path_timeout); + read_param_bool_timeout(&s->dragon_ui_blinker, "DragonUIBlinker", &s->dragon_ui_blinker_timeout); pthread_mutex_unlock(&s->lock);