Merge pull request commaai#537 from rav4kumar/071-clean

colored mpc track

selfdrive/controls/lib/pathplanner.py

@@ -13,7 +13,6 @@
 LaneChangeState = log.PathPlan.LaneChangeState
 LaneChangeDirection = log.PathPlan.LaneChangeDirection
 
-
 LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
 LANE_CHANGE_TIME_MAX = 10.
 
@@ -57,7 +56,7 @@ def __init__(self, CP):
     self.lane_change_state = LaneChangeState.off
     self.lane_change_timer = 0.0
     self.prev_one_blinker = False
-    
+
     self.op_params = opParams()
     self.alca_nudge_required = self.op_params.get('alca_nudge_required', default=True)
     self.alca_min_speed = self.op_params.get('alca_min_speed', default=20.0)
@@ -226,7 +225,6 @@ def update(self, sm, pm, CP, VM):
 
     pm.send('pathPlan', plan_send)
 
-
     dat = messaging.new_message()
     dat.init('liveMpc')
     dat.liveMpc.x = list(self.mpc_solution[0].x)

selfdrive/ui/SConscript

@@ -1,6 +1,6 @@
 Import('env', 'arch', 'common', 'messaging', 'gpucommon', 'visionipc', 'cereal')
 
-src = ['ui.cc', 'paint.cc', '#phonelibs/nanovg/nanovg.c']
+src = ['ui.cc', 'paint.cc', '#phonelibs/nanovg/nanovg.c', '#cereal/gen/c/log.capnp.c', '#cereal/gen/c/car.capnp.c']
 libs = [common, 'zmq', 'czmq', 'capnp', 'capnp_c', 'm', cereal, 'json', messaging, 'OpenCL', gpucommon, visionipc]
 
 if arch == "aarch64":
@@ -9,7 +9,7 @@ if arch == "aarch64":
 else:
   src += ['linux.cc']
   libs += ['EGL', 'pthread', 'X11-xcb', 'xcb', 'X11', 'glfw']
-  
+
 env.Program('_ui', src,
   LINKFLAGS=['-Wl,-rpath=/system/lib64,-rpath=/system/comma/usr/lib'],
   LIBS=libs)

selfdrive/ui/paint.cc

@@ -240,13 +240,21 @@ const UIScene *scene = &s->scene;
   NVGpaint track_bg;
   if (is_mpc) {
     // Draw colored MPC track
-    const uint8_t *clr = bg_colors[s->status];
-    track_bg = nvgLinearGradient(s->vg, vwp_w, vwp_h, vwp_w, vwp_h*.4,
-      nvgRGBA(clr[0], clr[1], clr[2], 255), nvgRGBA(clr[0], clr[1], clr[2], 255/2));
+    if (scene->steerOverride) {
+      track_bg = nvgLinearGradient(s->vg, vwp_w, vwp_h, vwp_w, vwp_h*.4,
+        nvgRGBA(0, 191, 255, 255), nvgRGBA(0, 95, 128, 50));
+    } else {
+      int torque_scale = (int)fabs(510*(float)s->scene.output_scale);
+      int red_lvl = fmin(255, torque_scale);
+      int green_lvl = fmin(255, 510-torque_scale);
+      track_bg = nvgLinearGradient(s->vg, vwp_w, vwp_h, vwp_w, vwp_h*.4,
+        nvgRGBA(          red_lvl,            green_lvl,  0, 255),
+        nvgRGBA((int)(0.5*red_lvl), (int)(0.5*green_lvl), 0, 50));
+    }
   } else {
     // Draw white vision track
     track_bg = nvgLinearGradient(s->vg, vwp_w, vwp_h, vwp_w, vwp_h*.4,
-      nvgRGBA(255, 255, 255, 255), nvgRGBA(255, 255, 255, 0));
+      nvgRGBA(255, 255, 255, 200), nvgRGBA(255, 255, 255, 50));
   }
 
   nvgFillPaint(s->vg, track_bg);
@@ -672,7 +680,7 @@ static void ui_draw_vision_event(UIState *s) {
     const int img_wheel_y = bg_wheel_y-25;
     const float img_rotation = s->scene.angleSteers/180*3.141592;
     float img_wheel_alpha = 0.1f;
-    bool is_engaged = (s->status == STATUS_ENGAGED);
+    bool is_engaged = (s->status == STATUS_ENGAGED) && !scene->steerOverride;
     bool is_warning = (s->status == STATUS_WARNING);
     bool is_engageable = scene->engageable;
     if (is_engaged || is_warning || is_engageable) {

selfdrive/ui/ui.cc

@@ -116,13 +116,16 @@ static void ui_init(UIState *s) {
   s->livecalibration_sock = SubSocket::create(s->ctx, "liveCalibration");
   s->radarstate_sock = SubSocket::create(s->ctx, "radarState");
   s->carstate_sock = SubSocket::create(s->ctx, "carState");
+  s->livempc_sock = SubSocket::create(s->ctx, "liveMpc");
   s->thermal_sock = SubSocket::create(s->ctxarne182, "thermalonline");
 
   assert(s->model_sock != NULL);
   assert(s->controlsstate_sock != NULL);
   assert(s->uilayout_sock != NULL);
   assert(s->livecalibration_sock != NULL);
   assert(s->radarstate_sock != NULL);
+  assert(s->carstate_sock != NULL);
+  assert(s->livempc_sock != NULL);
   assert(s->thermal_sock != NULL);
 
   s->poller = Poller::create({
@@ -131,7 +134,8 @@ static void ui_init(UIState *s) {
                               s->uilayout_sock,
                               s->livecalibration_sock,
                               s->radarstate_sock,
-                              s->carstate_sock
+                              s->carstate_sock,
+                              s->livempc_sock
                              });
   s->pollerarne182 = Poller::create({
                               s->thermal_sock
@@ -329,6 +333,15 @@ void handle_message(UIState *s, Message * msg) {
     struct cereal_ControlsState datad;
     cereal_read_ControlsState(&datad, eventd.controlsState);
 
+    struct cereal_ControlsState_LateralPIDState pdata;
+    cereal_read_ControlsState_LateralPIDState(&pdata, datad.lateralControlState.pidState);
+
+    struct cereal_ControlsState_LateralLQRState qdata;
+    cereal_read_ControlsState_LateralLQRState(&qdata, datad.lateralControlState.lqrState);
+
+    struct cereal_ControlsState_LateralINDIState rdata;
+    cereal_read_ControlsState_LateralINDIState(&rdata, datad.lateralControlState.indiState);
+
     s->controls_timeout = 1 * UI_FREQ;
     s->controls_seen = true;
 
@@ -344,6 +357,8 @@ void handle_message(UIState *s, Message * msg) {
     s->scene.engageable = datad.engageable;
     s->scene.gps_planner_active = datad.gpsPlannerActive;
     s->scene.monitoring_active = datad.driverMonitoringOn;
+    s->scene.steerOverride = datad.steerOverride;
+    s->scene.output_scale = qdata.output + rdata.output + pdata.output;
 
     s->scene.frontview = datad.rearViewCam;
 

selfdrive/ui/ui.hpp

@@ -122,6 +122,10 @@ typedef struct UIScene {
 
   bool recording;
 
+  // gernby pathcoloring
+  float output_scale;
+  bool steerOverride;
+
   // Used to show gps planner status
   bool gps_planner_active;
 
@@ -187,6 +191,7 @@ typedef struct UIState {
   SubSocket *livecalibration_sock;
   SubSocket *radarstate_sock;
   SubSocket *carstate_sock;
+  SubSocket *livempc_sock;
   SubSocket *map_data_sock;
   SubSocket *uilayout_sock;
   Poller * poller;