move e2e out of arne182.capnp (commaai#740)

* move e2e button out of arne182.capnp

* duplicate

* fixes

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;
   }
 }

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;
   }
 }

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.]

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

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),
   },
-
 }

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:

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'])
 

selfdrive/ui/android/ui.cc

@@ -2,6 +2,7 @@
 #include <stdlib.h>
 #include <math.h>
 #include <sys/resource.h>
+#include <iostream>
 
 #include <algorithm>
 
@@ -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) {

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,

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<char>(op_params_file)),
+                                //(std::istreambuf_iterator<char>()));
+
+  //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")) {

selfdrive/ui/ui.hpp

@@ -89,6 +89,8 @@ static std::map<UIStatus, NVGcolor> 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);