diff --git a/cereal/arne182.capnp b/cereal/arne182.capnp index 176522e62b3554..809b30a99f0334 100644 --- a/cereal/arne182.capnp +++ b/cereal/arne182.capnp @@ -167,10 +167,6 @@ struct TrafficModelEvent { confidence @1 :Float32; } -struct ModelLongButton { - enabled @0 :Bool; -} - struct EventArne182 { # in nanoseconds? logMonoTime @0 :UInt64; @@ -189,7 +185,5 @@ struct EventArne182 { trafficModelRaw @11: TrafficModelRaw; trafficModelEvent @12: TrafficModelEvent; dynamicFollowData @13 :DynamicFollowData; - #e2e button from shane. https://github.com/ShaneSmiskol/openpilot/pull/130/files - modelLongButton @14 :ModelLongButton; } } diff --git a/cereal/log.capnp b/cereal/log.capnp index cb1d69e050e25b..3cd9a1e22e0dca 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2056,6 +2056,10 @@ struct Sentinel { type @0 :SentinelType; } +struct ModelLongButton { + enabled @0 :Bool; +} + struct Event { # in nanoseconds? logMonoTime @0 :UInt64; @@ -2136,5 +2140,7 @@ struct Event { sentinel @73 :Sentinel; wideFrame @74: FrameData; modelV2 @75 :ModelDataV2; + #e2e button from shane. https://github.com/ShaneSmiskol/openpilot/pull/130/files + modelLongButton @76 :ModelLongButton; } } diff --git a/cereal/service_list.yaml b/cereal/service_list.yaml index d47300072fb759..d9cf26116dde3d 100644 --- a/cereal/service_list.yaml +++ b/cereal/service_list.yaml @@ -80,6 +80,7 @@ offroadLayout: [8074, false, 0.] wideEncodeIdx: [8075, true, 20.] wideFrame: [8076, true, 20.] modelV2: [8077, true, 20., 20] +modelLongButton: [8078, false, 0.] testModel: [8040, false, 0.] testLiveLocation: [8045, false, 0.] @@ -92,7 +93,6 @@ latControl: [8210, false, 20.] phantomData: [8211, false, 20.] thermalonline: [8212, false, 1., 1] ipAddress: [8213, false, 0.] -modelLongButton: [8214, false, 0.] #alca Events alcaStatus: [8214, false, 0.] diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index a95c030fc85df4..33aeadea7ee7b6 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -60,10 +60,10 @@ def __init__(self, sm=None, pm=None, can_sock=None, arne_sm=None): self.sm = sm if self.sm is None: self.sm = messaging.SubMaster(['thermal', 'health', 'frame', 'model', 'liveCalibration', - 'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman', 'radarState']) + 'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman', 'modelLongButton' , 'radarState']) self.arne_sm = arne_sm if self.arne_sm is None: - self.arne_sm = messaging_arne.SubMaster(['arne182Status', 'dynamicFollowButton', 'trafficModelEvent', 'modelLongButton' ]) + self.arne_sm = messaging_arne.SubMaster(['arne182Status', 'dynamicFollowButton', 'trafficModelEvent']) self.op_params = opParams() self.df_manager = dfManager(self.op_params) @@ -493,13 +493,13 @@ def publish_logs(self, CS, start_time, actuators, v_acc, a_acc, lac_log, CS_arne clear_event = ET.WARNING if ET.WARNING not in self.current_alert_types else None alerts = self.events.create_alerts(self.current_alert_types, [self.CP, self.sm, self.is_metric]) alertsArne182 = self.eventsArne182.create_alerts(self.current_alert_types, [self.CP, self.sm, self.is_metric]) - self.last_model_long = self.arne_sm['modelLongButton'].enabled + self.last_model_long = self.sm['modelLongButton'].enabled self.AM.add_many(self.sm.frame, alerts, self.enabled) self.AM.add_many(self.sm.frame, alertsArne182, self.enabled) df_out = self.df_manager.update() frame = self.sm.frame - if self.arne_sm['modelLongButton'].enabled != self.last_model_long: + if self.sm['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 diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 47cf7ecd976aaf..e8059e92d8dee4 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -973,12 +973,11 @@ def wrong_car_mode_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: boo ET.NO_ENTRY: NoEntryAlert("Reverse Gear"), }, - "modelLongAlert": { - ET.WARNING: Alert( + 'modelLongAlert': { + ET.PERMANENT: Alert( "Model longitudinal ", "Remain alert", AlertStatus.normal, AlertSize.mid, - Priority.LOW, VisualAlert.none, AudibleAlert.chimeWarning1, .4, 0., 2.), + Priority.LOW, VisualAlert.none, AudibleAlert.chimeWarning1, .4, 0., 1.5), }, - } diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index f64d13319ac65f..90ad193875e8c2 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -353,7 +353,7 @@ def update(self, sm, pm, CP, VM, PP, arne_sm): sm['model'].longitudinal.speeds, sm['model'].longitudinal.accelerations) - self.choose_solution(v_cruise_setpoint, enabled, lead_1, lead_2, sm['carState'].steeringAngle, arne_sm['modelLongButton'].enabled) + self.choose_solution(v_cruise_setpoint, enabled, lead_1, lead_2, sm['carState'].steeringAngle, sm['modelLongButton'].enabled) # determine fcw if self.mpc1.new_lead: diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index da0946dc015b7d..50c625bb262885 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -23,10 +23,10 @@ def plannerd_thread(sm=None, pm=None, arne_sm=None): VM = VehicleModel(CP) if sm is None: - sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters', 'liveMapData'], + sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters', 'modelLongButton', 'liveMapData'], poll=['radarState', 'model']) if arne_sm is None: - arne_sm = messaging_arne.SubMaster(['arne182Status', 'latControl', 'modelLongButton']) + arne_sm = messaging_arne.SubMaster(['arne182Status', 'latControl']) if pm is None: pm = messaging.PubMaster(['plan', 'liveLongitudinalMpc', 'pathPlan', 'liveMpc']) diff --git a/selfdrive/ui/android/ui.cc b/selfdrive/ui/android/ui.cc index 3cdc7ff40d0bd4..b882f6b3b024d4 100644 --- a/selfdrive/ui/android/ui.cc +++ b/selfdrive/ui/android/ui.cc @@ -2,6 +2,7 @@ #include #include #include +#include #include @@ -30,6 +31,41 @@ static void ui_set_brightness(UIState *s, int brightness) { } } +static void send_ml(UIState *s, bool enabled) { + MessageBuilder msg; + auto mlStatus = msg.initEvent().initModelLongButton(); + mlStatus.setEnabled(enabled); + s->pm->send("modelLongButton", msg); +} + +static bool handle_ml_touch(UIState *s, int touch_x, int touch_y) { + //mlButton manager + 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); + printf("ml button: %d\n", s->scene.mlButtonEnabled); + return true; + } + return false; +} + +static bool handle_SA_touched(UIState *s, int touch_x, int touch_y) { + if (s->active_app == cereal::UiLayoutState::App::NONE) { // if onroad (not settings or home) + if ((s->awake && s->vision_connected && s->status != STATUS_OFFROAD) || s->ui_debug) { // if car started or debug mode + if (handle_ml_touch(s, touch_x, touch_y)) { + s->scene.uilayout_sidebarcollapsed = true; // collapse sidebar when tapping any SA button + return true; // only allow one button to be pressed at a time + } + } + } + return false; +} + static void handle_display_state(UIState *s, bool user_input) { static int awake_timeout = 0; @@ -121,6 +157,7 @@ int main(int argc, char* argv[]) { UIState uistate = {}; UIState *s = &uistate; ui_init(s); + sa_init(s, true); s->sound = &sound; TouchState touch = {0}; @@ -147,10 +184,15 @@ int main(int argc, char* argv[]) { const int MAX_VOLUME = LEON ? 15 : 12; s->sound->setVolume(MIN_VOLUME); + bool last_started = s->started; while (!do_exit) { if (!s->started) { usleep(50 * 1000); } + if (s->started && !last_started) { + sa_init(s, false); // reset ml button and regrab params + } + last_started = s->started; double u1 = millis_since_boot(); ui_update(s); @@ -159,8 +201,11 @@ 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); } handle_sidebar_touch(s, touch_x, touch_y); - handle_vision_touch(s, touch_x, touch_y); + if (!handle_SA_touched(s, touch_x, touch_y)) { // if SA button not touched + handle_vision_touch(s, touch_x, touch_y); + } } if (s->awake) { diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 52729974f9c1b0..5374020a7d854f 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -659,6 +659,34 @@ static void ui_draw_driver_view(UIState *s) { ui_draw_circle_image(s->vg, x2, y2+border_shifter+25, brake_size-5, s->img_brake, s->scene.brakeLights); } +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 Rect &viz_rect = s->scene.viz_rect; NVGpaint gradient = nvgLinearGradient(s->vg, viz_rect.x, @@ -985,6 +1013,7 @@ static void ui_draw_vision_footer(UIState *s) { ui_draw_vision_brake(s); bb_ui_draw_UI(s); ui_draw_vision_map(s); + ui_draw_ml_button(s); } void ui_draw_vision_alert(UIState *s, cereal::ControlsState::AlertSize va_size, UIStatus va_color, diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index f1ddf6c0072adb..1cb35e98b9c56f 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -23,6 +23,34 @@ int write_param_float(float param, const char* param_name, bool persistent_param return Params(persistent_param).write_db_value(param_name, s, size < sizeof(s) ? size : sizeof(s)); } +void sa_init(UIState *s, bool full_init) { + if (full_init) { + s->pm = new PubMaster({"modelLongButton"}); + } + + s->ui_debug = false; // change to true while debugging + + // stock additions todo: run opparams first (in main()?) to ensure json values exist + //std::ifstream op_params_file("/data/op_params.json"); + //std::string op_params_content((std::istreambuf_iterator(op_params_file)), + //(std::istreambuf_iterator())); + + //std::string err; + //auto json = json11::Json::parse(op_params_content, err); + //if (!json.is_null() && err.empty()) { + // printf("successfully parsed opParams json\n"); + // s->scene.dfButtonStatus = DF_TO_IDX[json["dynamic_follow"].string_value()]; + //s->scene.lsButtonStatus = LS_TO_IDX[json["lane_speed_alerts"].string_value()]; +// printf("dfButtonStatus: %d\n", s->scene.dfButtonStatus); +// printf("lsButtonStatus: %d\n", s->scene.lsButtonStatus); + //} else { // error parsing json + // printf("ERROR PARSING OPPARAMS JSON!\n"); + // s->scene.dfButtonStatus = 0; + // s->scene.lsButtonStatus = 0; + //} + s->scene.mlButtonEnabled = false; // state isn't saved yet +} + void ui_init(UIState *s) { s->sm = new SubMaster({"model", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal", "liveMapData", "health", "carParams", "ubloxGnss", "driverState", "dMonitoringState", "sensorEvents", "carState", "liveMpc", "gpsLocationExternal"}); @@ -215,10 +243,10 @@ void update_sockets(UIState *s) { s->scene.cpuPerc = scene.thermal.getCpuPerc(); } if (sm.updated("liveMapData")) { - scene.map_valid = sm["liveMapData"].getLiveMapData().getMapValid(); - scene.speedlimit = sm["liveMapData"].getLiveMapData().getSpeedLimit(); - scene.speedlimit_valid = sm["liveMapData"].getLiveMapData().getSpeedLimitValid(); - scene.speedlimitahead_valid = sm["liveMapData"].getLiveMapData().getSpeedLimitAheadValid(); + scene.map_valid = sm["liveMapData"].getLiveMapData().getMapValid(); + scene.speedlimit = sm["liveMapData"].getLiveMapData().getSpeedLimit(); + scene.speedlimit_valid = sm["liveMapData"].getLiveMapData().getSpeedLimitValid(); + scene.speedlimitahead_valid = sm["liveMapData"].getLiveMapData().getSpeedLimitAheadValid(); scene.speedlimitaheaddistance = sm["liveMapData"].getLiveMapData().getSpeedLimitAheadDistance(); } if (sm.updated("ubloxGnss")) { diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index 29146a3d89f4b2..ccb7e24aa83e2f 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -89,6 +89,8 @@ static std::map bg_colors = { typedef struct UIScene { + bool mlButtonEnabled; + float mpc_x[50]; float mpc_y[50]; @@ -199,6 +201,7 @@ typedef struct UIState { int img_speed; SubMaster *sm; + PubMaster *pm; Sound *sound; UIStatus status; @@ -228,6 +231,7 @@ typedef struct UIState { bool ignition; bool is_metric; bool longitudinal_control; + bool ui_debug; bool limit_set_speed; bool is_ego_over_limit; float speed_lim_off; @@ -244,6 +248,7 @@ typedef struct UIState { } UIState; void ui_init(UIState *s); +void sa_init(UIState *s, bool full_init); void ui_update(UIState *s); int write_param_float(float param, const char* param_name, bool persistent_param = false);