Add a toggle to enable model long

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

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

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,24 +278,29 @@ 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
     if faster_lane in ['left', 'right']:
       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
 

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

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(

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:

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

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

selfdrive/ui/ui

@@ -1,4 +1,3 @@
 #!/bin/sh
 export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH"
-exec ./_ui
-
+exec ./_ui "$@"