diff --git a/cereal/log.capnp b/cereal/log.capnp index d1b16bff7905fa..215cfa8127aa19 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2014,6 +2014,10 @@ struct DynamicCameraOffset { keepingRight @1 :Bool; } +struct ModelLongButton { + enabled @0 :Bool; +} + struct Event { # in nanoseconds? logMonoTime @0 :UInt64; @@ -2098,5 +2102,6 @@ struct Event { laneSpeed @76 :LaneSpeed; laneSpeedButton @77 :LaneSpeedButton; dynamicCameraOffset @78 :DynamicCameraOffset; + modelLongButton @79 :ModelLongButton; } } diff --git a/cereal/service_list.yaml b/cereal/service_list.yaml index 0bc29df212765e..466489740c75a6 100644 --- a/cereal/service_list.yaml +++ b/cereal/service_list.yaml @@ -83,6 +83,7 @@ dynamicFollowButton: [8076, false, 0.] laneSpeed: [8077, false, 0.] laneSpeedButton: [8078, false, 0.] dynamicCameraOffset: [8079, false, 0.] +modelLongButton: [8080, false, 0.] testModel: [8040, false, 0.] testLiveLocation: [8045, false, 0.] diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index e2163274830af3..d68f5cb1f65682 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -55,11 +55,13 @@ def __init__(self, sm=None, pm=None, can_sock=None): if self.sm is None: self.sm = messaging.SubMaster(['thermal', 'health', 'frame', 'model', 'liveCalibration', 'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman']) - self.sm_smiskol = messaging.SubMaster(['radarState', 'dynamicFollowData', 'liveTracks', 'dynamicFollowButton', 'laneSpeed', 'dynamicCameraOffset']) + self.sm_smiskol = messaging.SubMaster(['radarState', 'dynamicFollowData', 'liveTracks', 'dynamicFollowButton', + 'laneSpeed', 'dynamicCameraOffset', 'modelLongButton']) self.op_params = opParams() self.df_manager = dfManager(self.op_params) - self.hide_auto_df_alerts = self.op_params.get('hide_auto_df_alerts', False) + self.hide_auto_df_alerts = self.op_params.get('hide_auto_df_alerts', False) + self.last_model_long = False self.can_sock = can_sock if can_sock is None: @@ -276,16 +278,21 @@ def data_sample(self): def add_stock_additions_alerts(self, CS): frame = self.sm.frame # alert priority is defined by code location, keeping is highest, then lane speed alert, then auto-df alert + if self.sm_smiskol['modelLongButton'].enabled != self.last_model_long: + extra_text_1 = 'disabled!' if self.last_model_long else 'enabled!' + self.AM.add_custom(frame, 'modelLongAlert', ET.WARNING, self.enabled, extra_text_1=extra_text_1) + return + if self.sm_smiskol['dynamicCameraOffset'].keepingLeft: - self.AM.add_custom(frame, 'laneSpeedKeeping', self.enabled, extra_text_1='LEFT', extra_text_2='Oncoming traffic in right lane') + self.AM.add_custom(frame, 'laneSpeedKeeping', ET.WARNING, self.enabled, extra_text_1='LEFT', extra_text_2='Oncoming traffic in right lane') return elif self.sm_smiskol['dynamicCameraOffset'].keepingRight: - self.AM.add_custom(frame, 'laneSpeedKeeping', self.enabled, extra_text_1='RIGHT', extra_text_2='Oncoming traffic in left lane') + self.AM.add_custom(frame, 'laneSpeedKeeping', ET.WARNING, self.enabled, extra_text_1='RIGHT', extra_text_2='Oncoming traffic in left lane') return ls_state = self.sm_smiskol['laneSpeed'].state if ls_state != '': - self.AM.add_custom(frame, 'lsButtonAlert', self.enabled, extra_text_1=ls_state) + self.AM.add_custom(frame, 'lsButtonAlert', ET.WARNING, self.enabled, extra_text_1=ls_state) return faster_lane = self.sm_smiskol['laneSpeed'].fastestLane @@ -293,7 +300,7 @@ def add_stock_additions_alerts(self, CS): ls_alert = 'laneSpeedAlert' if not self.sm_smiskol['laneSpeed'].new: ls_alert += 'Silent' - self.AM.add_custom(frame, ls_alert, self.enabled, extra_text_1='{} lane faster'.format(faster_lane).upper(), extra_text_2='Change lanes to faster {} lane'.format(faster_lane)) + self.AM.add_custom(frame, ls_alert, ET.WARNING, self.enabled, extra_text_1='{} lane faster'.format(faster_lane).upper(), extra_text_2='Change lanes to faster {} lane'.format(faster_lane)) return df_out = self.df_manager.update() @@ -303,10 +310,10 @@ def add_stock_additions_alerts(self, CS): # only show auto alert if engaged, not hiding auto, and time since lane speed alert not showing if CS.cruiseState.enabled and not self.hide_auto_df_alerts: df_alert += 'Silent' - self.AM.add_custom(frame, df_alert, self.enabled, extra_text_1=df_out.model_profile_text + ' (auto)') + self.AM.add_custom(frame, df_alert, ET.WARNING, self.enabled, extra_text_1=df_out.model_profile_text + ' (auto)') return else: - self.AM.add_custom(frame, df_alert, self.enabled, extra_text_1=df_out.user_profile_text, extra_text_2='Dynamic follow: {} profile active'.format(df_out.user_profile_text)) + self.AM.add_custom(frame, df_alert, ET.WARNING, self.enabled, extra_text_1=df_out.user_profile_text, extra_text_2='Dynamic follow: {} profile active'.format(df_out.user_profile_text)) return def state_transition(self, CS): @@ -486,6 +493,7 @@ def publish_logs(self, CS, start_time, actuators, v_acc, a_acc, lac_log): alerts = self.events.create_alerts(self.current_alert_types, [self.CP, self.sm, self.is_metric]) self.AM.add_many(self.sm.frame, alerts, self.enabled) self.add_stock_additions_alerts(CS) + self.last_model_long = self.sm_smiskol['modelLongButton'].enabled self.AM.process_alerts(self.sm.frame) CC.hudControl.visualAlert = self.AM.visual_alert diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index 4e6e47e43aef8a..d38164e9974269 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/controls/lib/alertmanager.py @@ -22,8 +22,8 @@ def add_many(self, frame, alerts, enabled=True): for a in alerts: self.add(frame, a, enabled=enabled) - def add_custom(self, frame, alert_name, enabled=True, extra_text_1='', extra_text_2=''): - alert = EVENTS[alert_name] + def add_custom(self, frame, alert_name, event_type, enabled=True, extra_text_1='', extra_text_2=''): + alert = EVENTS[alert_name][event_type] added_alert = copy.copy(alert) added_alert.start_time = frame * DT_CTRL added_alert.alert_text_1 += extra_text_1 diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index d3a5b35e2626ca..af0eacd4ce2060 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -525,41 +525,61 @@ def wrong_car_mode_alert(CP, sm, metric): duration_hud_alert=0.), }, - "dfButtonAlert": Alert( - "Using profile: ", - "", - AlertStatus.normal, AlertSize.mid, - Priority.LOW, VisualAlert.none, AudibleAlert.chimeWarning1, 0.2, 0., 2.), - - "lsButtonAlert": Alert( - "Lane Speed set to: ", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.chimeWarning1, 0.2, 0., 2.), - - "dfButtonAlertSilent": Alert( - "Dynamic follow: ", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0.2, 0., 2.), - - "laneSpeedAlert": Alert( - "", - "", - AlertStatus.normal, AlertSize.mid, - Priority.LOW, VisualAlert.none, AudibleAlert.chimeWarning1, 0.2, 0., 0.1), + "modelLongAlert": { + ET.WARNING: Alert( + "Model longitudinal ", + "Remain alert", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeWarning1, .4, 0., 2.), + }, - "laneSpeedAlertSilent": Alert( - "", - "", - AlertStatus.normal, AlertSize.mid, - Priority.LOW, VisualAlert.none, AudibleAlert.none, 0.2, 0., 0.1), + "dfButtonAlert": { + ET.WARNING: Alert( + "Using profile: ", + "", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeWarning1, .4, 0., 2.), + }, - "laneSpeedKeeping": Alert( - "KEEPING ", - "", - AlertStatus.normal, AlertSize.mid, - Priority.LOW, VisualAlert.none, AudibleAlert.none, 0.2, 0., 0.1), + "lsButtonAlert": { + ET.WARNING: Alert( + "Lane Speed set to: ", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeWarning1, .4, 0., 2.), + }, + + "dfButtonAlertSilent": { + ET.WARNING: Alert( + "Dynamic follow: ", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOWER, VisualAlert.none, AudibleAlert.none, .2, 0., 2.), + }, + + "laneSpeedAlert": { + ET.WARNING: Alert( + "", + "", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeWarning1, .4, 0., .1), + }, + + "laneSpeedAlertSilent": { + ET.WARNING: Alert( + "", + "", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .2, 0., .1), + }, + + "laneSpeedKeeping": { + ET.WARNING: Alert( + "KEEPING ", + "", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 0.2, 0., 0.1), + }, EventName.posenetInvalid: { ET.WARNING: Alert( diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 4eae35bb347f0f..53366cca2ae726 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -14,6 +14,7 @@ from selfdrive.controls.lib.fcw import FCWChecker from selfdrive.controls.lib.long_mpc import LongitudinalMpc from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX +from selfdrive.controls.lib.long_mpc_model import LongitudinalMpcModel MAX_SPEED = 255.0 @@ -69,6 +70,7 @@ def __init__(self, CP): self.mpc1 = LongitudinalMpc(1) self.mpc2 = LongitudinalMpc(2) + self.mpc_model = LongitudinalMpcModel() self.v_acc_start = 0.0 self.a_acc_start = 0.0 @@ -86,13 +88,15 @@ def __init__(self, CP): self.params = Params() self.first_loop = True - def choose_solution(self, v_cruise_setpoint, enabled): + def choose_solution(self, v_cruise_setpoint, enabled, model_enabled): if enabled: solutions = {'cruise': self.v_cruise} if self.mpc1.prev_lead_status: solutions['mpc1'] = self.mpc1.v_mpc if self.mpc2.prev_lead_status: solutions['mpc2'] = self.mpc2.v_mpc + if self.mpc_model.valid and model_enabled: + solutions['model'] = self.mpc_model.v_mpc slowest = min(solutions, key=solutions.get) @@ -107,8 +111,11 @@ def choose_solution(self, v_cruise_setpoint, enabled): elif slowest == 'cruise': self.v_acc = self.v_cruise self.a_acc = self.a_cruise + elif slowest == 'model': + self.v_acc = self.mpc_model.v_mpc + self.a_acc = self.mpc_model.a_mpc - self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint]) + self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, self.mpc_model.v_mpc_future, v_cruise_setpoint]) def update(self, sm, pm, CP, VM, PP): """Gets called when new radarState is available""" @@ -161,11 +168,16 @@ def update(self, sm, pm, CP, VM, PP): self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) + self.mpc_model.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc1.update(pm, sm['carState'], lead_1, v_cruise_setpoint) self.mpc2.update(pm, sm['carState'], lead_2, v_cruise_setpoint) + self.mpc_model.update(sm['carState'].vEgo, sm['carState'].aEgo, + sm['model'].longitudinal.distances, + sm['model'].longitudinal.speeds, + sm['model'].longitudinal.accelerations) - self.choose_solution(v_cruise_setpoint, enabled) + self.choose_solution(v_cruise_setpoint, enabled, sm['modelLongButton'].enabled) # determine fcw if self.mpc1.new_lead: diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index b439681a3a2303..5910c11f756474 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -27,7 +27,7 @@ def plannerd_thread(sm=None, pm=None): VM = VehicleModel(CP) if sm is None: - sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters']) + sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters', 'modelLongButton']) if pm is None: pm = messaging.PubMaster(['plan', 'liveLongitudinalMpc', 'pathPlan', 'liveMpc']) diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 9cd5871250ee51..491430409582c4 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -619,8 +619,10 @@ static void ui_draw_driver_view(UIState *s) { static void ui_draw_ls_button(UIState *s) { int btn_w = 150; int btn_h = 150; - int btn_x = 1920 - btn_w - 200; // 150 + 50 padding - int btn_y = 1080 - btn_h - 50; + int x_padding = 200; + int y_padding = 50; + int btn_x = 1920 - btn_w - x_padding; + int btn_y = 1080 - btn_h - y_padding; nvgBeginPath(s->vg); nvgRoundedRect(s->vg, btn_x-110, btn_y-45, btn_w, btn_h, 100); @@ -640,8 +642,9 @@ static void ui_draw_ls_button(UIState *s) { static void ui_draw_df_button(UIState *s) { int btn_w = 150; int btn_h = 150; + int y_padding = 50; int btn_x = 1920 - btn_w; - int btn_y = 1080 - btn_h - 50; + int btn_y = 1080 - btn_h - y_padding; nvgBeginPath(s->vg); nvgRoundedRect(s->vg, btn_x-110, btn_y-45, btn_w, btn_h, 100); @@ -658,6 +661,34 @@ static void ui_draw_df_button(UIState *s) { nvgText(s->vg, btn_x - 34, btn_y + 50 + 15, "profile", NULL); } +static void ui_draw_ml_button(UIState *s) { + int btn_w = 500; + int btn_h = 138; + int x = 1920 / 2; + int y = 915; + int btn_x = x - btn_w / 2; + int btn_y = y - btn_h / 2; + + nvgBeginPath(s->vg); + nvgRoundedRect(s->vg, btn_x, btn_y, btn_w, btn_h, 25); + if (s->scene.mlButtonEnabled) { // change outline color based on status of button + nvgStrokeColor(s->vg, nvgRGBA(55, 184, 104, 255)); + } else { + nvgStrokeColor(s->vg, nvgRGBA(184, 55, 55, 255)); + } + nvgStrokeWidth(s->vg, 12); + nvgStroke(s->vg); + + nvgBeginPath(s->vg); // dark background for readability + nvgRoundedRect(s->vg, btn_x, btn_y, btn_w, btn_h, 25); + nvgFillColor(s->vg, nvgRGBA(75, 75, 75, 75)); + nvgFill(s->vg); + + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); + nvgFontSize(s->vg, 65); + nvgText(s->vg, x, y + btn_h / 8, "Toggle Model Long", NULL); +} + static void ui_draw_vision_header(UIState *s) { const UIScene *scene = &s->scene; int ui_viz_rx = scene->ui_viz_rx; @@ -685,6 +716,7 @@ static void ui_draw_vision_footer(UIState *s) { ui_draw_vision_face(s); ui_draw_df_button(s); ui_draw_ls_button(s); + ui_draw_ml_button(s); #ifdef SHOW_SPEEDLIMIT // ui_draw_vision_map(s); diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index d212c92da72524..b663dce0f4bddb 100755 --- a/selfdrive/ui/ui +++ b/selfdrive/ui/ui @@ -1,4 +1,3 @@ #!/bin/sh export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH" -exec ./_ui - +exec ./_ui "$@" diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 675911e60499c1..e5aeb395a6a363 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -97,14 +97,31 @@ static void send_ls(UIState *s, int status) { s->pm->send("laneSpeedButton", msg); } +static void send_df(UIState *s, int status) { + capnp::MallocMessageBuilder msg; + auto event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + auto dfStatus = event.initDynamicFollowButton(); + dfStatus.setStatus(status); + s->pm->send("dynamicFollowButton", msg); +} + +static void send_ml(UIState *s, bool enabled) { + capnp::MallocMessageBuilder msg; + auto event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + auto mlStatus = event.initModelLongButton(); + mlStatus.setEnabled(enabled); + s->pm->send("modelLongButton", msg); +} + static bool handle_ls_touch(UIState *s, int touch_x, int touch_y) { //lsButton manager - if (s->awake && s->vision_connected && s->status != STATUS_STOPPED) { + if ((s->awake && s->vision_connected && s->status != STATUS_STOPPED) || s->ui_debug) { int padding = 40; int btn_x_1 = 1660 - 200; int btn_x_2 = 1660 - 50; if ((btn_x_1 - padding <= touch_x) && (touch_x <= btn_x_2 + padding) && (855 - padding <= touch_y)) { - s->scene.uilayout_sidebarcollapsed = true; // collapse sidebar when tapping ls button s->scene.lsButtonStatus++; if (s->scene.lsButtonStatus > 2) { s->scene.lsButtonStatus = 0; @@ -116,21 +133,11 @@ static bool handle_ls_touch(UIState *s, int touch_x, int touch_y) { return false; } -static void send_df(UIState *s, int status) { - capnp::MallocMessageBuilder msg; - auto event = msg.initRoot(); - event.setLogMonoTime(nanos_since_boot()); - auto dfStatus = event.initDynamicFollowButton(); - dfStatus.setStatus(status); - s->pm->send("dynamicFollowButton", msg); -} - static bool handle_df_touch(UIState *s, int touch_x, int touch_y) { //dfButton manager - if (s->awake && s->vision_connected && s->status != STATUS_STOPPED) { + if ((s->awake && s->vision_connected && s->status != STATUS_STOPPED) || s->ui_debug) { int padding = 40; if ((1660 - padding <= touch_x) && (855 - padding <= touch_y)) { - s->scene.uilayout_sidebarcollapsed = true; // collapse sidebar when tapping df button s->scene.dfButtonStatus++; if (s->scene.dfButtonStatus > 3) { s->scene.dfButtonStatus = 0; @@ -142,6 +149,23 @@ static bool handle_df_touch(UIState *s, int touch_x, int touch_y) { return false; } +static bool handle_ml_touch(UIState *s, int touch_x, int touch_y) { + //mlButton manager + if ((s->awake && s->vision_connected && s->status != STATUS_STOPPED) || s->ui_debug) { + int padding = 40; + int btn_w = 500; + int btn_h = 138; + int xs[2] = {1920 / 2 - btn_w / 2, 1920 / 2 + btn_w / 2}; + int y_top = 915 - btn_h / 2; + if (xs[0] <= touch_x + padding && touch_x - padding <= xs[1] && y_top - padding <= touch_y) { + s->scene.mlButtonEnabled = !s->scene.mlButtonEnabled; + send_ml(s, s->scene.mlButtonEnabled); + return true; + } + } + return false; +} + static void handle_sidebar_touch(UIState *s, int touch_x, int touch_y) { if (!s->scene.uilayout_sidebarcollapsed && touch_x <= sbr_w) { if (touch_x >= settings_btn_x && touch_x < (settings_btn_x + settings_btn_w) @@ -227,12 +251,13 @@ static void ui_init(UIState *s) { , "liveMapData" #endif }); - s->pm = new PubMaster({"offroadLayout", "laneSpeedButton", "dynamicFollowButton"}); + s->pm = new PubMaster({"offroadLayout", "laneSpeedButton", "dynamicFollowButton", "modelLongButton"}); s->ipc_fd = -1; s->scene.satelliteCount = -1; s->started = false; s->vision_seen = false; + s->ui_debug = false; // change to true while debugging // init display s->fb = framebuffer_init("ui", 0, true, &s->fb_w, &s->fb_h); @@ -271,7 +296,7 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs, s->scene.dfButtonStatus = 0; s->scene.lsButtonStatus = 0; - + s->scene.mlButtonEnabled = false; s->rgb_width = back_bufs.width; s->rgb_height = back_bufs.height; @@ -834,11 +859,16 @@ int main(int argc, char* argv[]) { int touch_x = -1, touch_y = -1; int touched = touch_poll(&touch, &touch_x, &touch_y, 0); if (touched == 1) { + if (s->ui_debug) { + printf("touched x: %d, y: %d\n", touch_x, touch_y); + } set_awake(s, true); handle_sidebar_touch(s, touch_x, touch_y); - if (!handle_df_touch(s, touch_x, touch_y) && !handle_ls_touch(s, touch_x, touch_y)) { // disables sidebar from popping out when tapping df or ls button + if (!handle_df_touch(s, touch_x, touch_y) && !handle_ls_touch(s, touch_x, touch_y) && !handle_ml_touch(s, touch_x, touch_y)) { // disables sidebar from popping out when tapping df or ls button handle_vision_touch(s, touch_x, touch_y); + } else { + s->scene.uilayout_sidebarcollapsed = true; // collapse sidebar when tapping any SA button } } diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index 6a2d6bfe6501d7..c74b8af5ba6943 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -126,6 +126,7 @@ typedef struct UIScene { int dfButtonStatus; int lsButtonStatus; + bool mlButtonEnabled; cereal::HealthData::HwType hwType; int satelliteCount; @@ -238,6 +239,7 @@ typedef struct UIState { bool started; bool preview_started; bool vision_seen; + bool ui_debug; std::atomic light_sensor;