Adjustable Follow Distance w/HUD (#12)

* TR

* TR

* TR

* TR

* TR

* TR

* TR

* TR

* remove readdistancelines - causes boot error

* copying cereal

* arne's cereal

* HUD distance lines display

* capnpp add

* revert

* revert

* Set ACC HUD to KPH

* Test - set HUD_ACC distance line to 2

* revert cruise_kph

LKAS error

* TR

* TR

* TR

* TR

* TR

* TR

* TR

* TR

* remove readdistancelines - causes boot error

* copying cereal

* HUD distance lines display

* capnpp add

* revert

* revert

* Set ACC HUD to KPH

* Test - set HUD_ACC distance line to 2

* revert cruise_kph

LKAS error

* Remove custom btn var line

could be crashing the code when button pressed

* HUD distance lines

* HUD distance lines

* passing CS.CP.readdistancelines

* Hardcode hud object dist_lines var

* Initializes CS.readdistancelines before passing to hud obj

* use integer read_distance_lines instead

* Add HUD_DISTANCE_3 for horizontal HUD bars on dash

* Default to 2 bars follow distance (1.8s)

cereal/Makefile

@@ -1,14 +1,14 @@
 PWD := $(shell pwd)
 
-SRCS := log.capnp car.capnp
+SRCS := log.capnp car.capnp ui.capnp
 
-GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++
-JS := gen/js/car.capnp.js gen/js/log.capnp.js
+GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++ gen/cpp/ui.capnp.c++
+JS := gen/js/car.capnp.js gen/js/log.capnp.js gen/js/ui.capnp.js
 
 UNAME_M ?= $(shell uname -m)
 # only generate C++ for docker tests
 ifneq ($(OPTEST),1)
-	GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/include/c++.capnp.h gen/c/include/java.capnp.h
+	GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/ui.capnp.c gen/c/include/c++.capnp.h gen/c/include/java.capnp.h
 
 	ifeq ($(UNAME_M),x86_64)
 		ifneq (, $(shell which capnpc-java))

cereal/__init__.py

@@ -6,3 +6,4 @@
 
 log = capnp.load(os.path.join(CEREAL_PATH, "log.capnp"))
 car = capnp.load(os.path.join(CEREAL_PATH, "car.capnp"))
+ui = capnp.load(os.path.join(CEREAL_PATH, "ui.capnp"))

cereal/car.capnp

@@ -116,6 +116,13 @@ struct CarState {
   leftBlinker @20 :Bool;
   rightBlinker @21 :Bool;
   genericToggle @23 :Bool;
+  blindspot @26 :Bool;
+  distanceToggle @27 :Float32;
+  laneDepartureToggle @28 :Bool;
+  accSlowToggle @29 :Bool;
+  blindspotside @30 :Float32;
+  readdistancelines @31 :Float32;
+  gasbuttonstatus @32 :Float32;
 
   # lock info
   doorOpen @24 :Bool;

cereal/ui.capnp

@@ -0,0 +1,64 @@
+using Cxx = import "./include/c++.capnp";
+$Cxx.namespace("cereal");
+
+using Java = import "./include/java.capnp";
+$Java.package("ai.comma.openpilot.cereal");
+$Java.outerClassname("Ui");
+
+using Car = import "car.capnp";
+
+@0xce6ca45dddcd5317;
+
+struct UIButtonInfo {
+    # button ID 0..5
+    btnId @0 :Int8;
+    # internal button name
+    btnName @1 :Text;
+    # display label for button (3 chars)
+    btnLabel @2 :Text;
+    # buttons status: 0 = DISABLED, 1 = AVAILABLE, 2 = ENABLED, 3 = WARNING, 9 = NOT AVAILABLE
+    btnStatus @3 :Int16;
+    # small font label shows below the main label, max 7 chars
+    btnLabel2 @4 :Text;
+}
+
+struct UIButtonStatus {
+    # button ID 0..5
+    btnId @0 :Int8;
+    # buttons status: 0 = DISABLED, 1 = AVAILABLE, 2 = ENABLED, 3 = WARNING, 9 = NOT AVAILABLE
+    btnStatus @1 :Int16;
+}
+
+struct UICustomAlert {
+    caStatus @0 :Int8;
+    caText @1 :Text;
+}
+
+struct UISetCar {
+    icCarFolder @0 :Text;
+    icCarName @1 :Text;
+}
+
+struct UIPlaySound {
+    sndSound @0 :Int8;
+}
+
+struct UIUpdate {
+    uiDoUpdate @0 :Int8;
+    uiStatus @1 :Int8;
+    uiCanDisplayMessage @2 :Int8;
+}
+
+#struct UIEvent {
+#    # in nanoseconds?
+#    logMonoTime @0 :UInt64;
+#
+#    union {
+#        uiButtonInfo @1 :UIButtonInfo;
+#        uiCustomAlert @2 :UICustomAlert;
+#        uiSetCar @3 :UISetCar;
+#        uiButtonStatus @4 :UIButtonStatus;
+#        uiUpdate @5 :UIUpdate;
+#        uiPlaySound @6 :UIPlaySound;
+#    }
+#}

selfdrive/car/honda/carcontroller.py

@@ -70,7 +70,7 @@ def process_hud_alert(hud_alert):
 
 HUDData = namedtuple("HUDData",
                      ["pcm_accel", "v_cruise", "mini_car", "car", "X4",
-                      "lanes", "beep", "chime", "fcw", "acc_alert", "steer_required"])
+                      "lanes", "beep", "chime", "fcw", "acc_alert", "steer_required", "dist_lines"])
 
 
 class CarController(object):
@@ -118,7 +118,12 @@ def update(self, sendcan, enabled, CS, frame, actuators, \
         hud_car = 1
     else:
       hud_car = 0
-
+
+    #if CS.readdistancelines == 0 or CS.readdistancelines == 1 or CS.readdistancelines == 2 or CS.readdistancelines == 3:
+    #  distance_lines = CS.readdistancelines
+    #else:
+    #  distance_lines = 1
+
     # For lateral control-only, send chimes as a beep since we don't send 0x1fa
     if CS.CP.radarOffCan:
       snd_beep = snd_beep if snd_beep is not 0 else snd_chime
@@ -127,7 +132,7 @@ def update(self, sendcan, enabled, CS, frame, actuators, \
     fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
 
     hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car,
-                  0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required)
+                  0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required, CS.read_distance_lines)
 
     # **** process the car messages ****
 

selfdrive/car/honda/carstate.py

@@ -143,6 +143,9 @@ def get_cam_can_parser(CP):
 
 class CarState(object):
   def __init__(self, CP):
+    self.trLabels = ["0.9","1.8","2.7"]
+    self.trMode = 1
+    self.read_distance_lines_prev = 3
     self.CP = CP
     self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
     self.shifter_values = self.can_define.dv["GEARBOX"]["GEAR_SHIFTER"]
@@ -183,7 +186,7 @@ def update(self, cp, cp_cam):
 
     # update prevs, update must run once per loop
     self.prev_cruise_buttons = self.cruise_buttons
-    self.prev_cruise_setting = self.cruise_setting
+    #self.prev_cruise_setting = self.cruise_setting
     self.prev_blinker_on = self.blinker_on
 
     self.prev_left_blinker_on = self.left_blinker_on
@@ -242,7 +245,7 @@ def update(self, cp, cp_cam):
     self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE']
     self.angle_steers_rate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE']
 
-    self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
+    #self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
     self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS']
 
     self.blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] or cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER']
@@ -303,6 +306,21 @@ def update(self, cp, cp_cam):
     self.user_brake = cp.vl["VSA_STATUS"]['USER_BRAKE']
     self.pcm_acc_status = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS']
     self.hud_lead = cp.vl["ACC_HUD"]['HUD_LEAD']
+
+    if self.cruise_setting == 3:
+      if cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"] == 0:
+        self.trMode = (self.trMode + 1 ) % 3
+    self.prev_cruise_setting = self.cruise_setting
+    self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
+    self.read_distance_lines = self.trMode + 1
+    if self.read_distance_lines <> self.read_distance_lines_prev:
+    #  if self.read_distance_lines == 1:
+    #    self.UE.custom_alert_message(2,"Following distance set to 0.9s",200,3)
+    #  if self.read_distance_lines == 2:
+    #    self.UE.custom_alert_message(2,"Following distance set to 1.8s",200,3)
+    #  if self.read_distance_lines == 3:
+    #    self.UE.custom_alert_message(2,"Following distance set to 2.7s",200,3)
+      self.read_distance_lines_prev = self.read_distance_lines
 
 
 # carstate standalone tester

selfdrive/car/honda/hondacan.py

@@ -77,9 +77,10 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, idx):
       'CRUISE_SPEED': hud.v_cruise,
       'ENABLE_MINI_CAR': hud.mini_car,
       'HUD_LEAD': hud.car,
-      'SET_ME_X03': 0x03,
+      'SET_ME_X03': hud.dist_lines,
       'SET_ME_X03_2': 0x03,
       'SET_ME_X01': 0x01,
+      'HUD_DISTANCE_3': 1,
     }
     commands.append(packer.make_can_msg("ACC_HUD", 0, acc_hud_values, idx))
 

selfdrive/car/honda/interface.py

@@ -432,6 +432,8 @@ def update(self, c):
     ret.cruiseState.available = bool(self.CS.main_on)
     ret.cruiseState.speedOffset = self.CS.cruise_speed_offset
     ret.cruiseState.standstill = False
+
+    ret.readdistancelines = self.CS.read_distance_lines
 
     # TODO: button presses
     buttonEvents = []

selfdrive/controls/lib/drive_helpers.py

@@ -18,7 +18,7 @@ class MPC_COST_LAT:
 
 class MPC_COST_LONG:
   TTC = 5.0
-  DISTANCE = 0.1
+  DISTANCE = 0.8
   ACCELERATION = 10.0
   JERK = 20.0
 

selfdrive/controls/lib/longitudinal_mpc/Makefile

@@ -72,7 +72,7 @@ lib_qp/%.o: $(PHONELIBS)/qpoases/SRC/%.cpp
            -c -o '$@' '$<'
 
 generator: generator.cpp
-	$(CXX) -Wall -std=c++11 \
+	$(CXX) -v -Wall -std=c++11 \
          generator.cpp \
          -o generator \
          $(ACADO_FLAGS) \

selfdrive/controls/lib/longitudinal_mpc/fixdistancevar.sh

@@ -0,0 +1,2 @@
+#!/usr/bin/env bash
+sed -i -e 's/1\.2345678[^)]*)/TR)/g' -e 's/out)/out, double TR)/g' -e 's/Objective(  )/Objective(double TR)/g' -e 's/ValueOut )/ValueOut, TR )/g' -e 's/ionStep(  )/ionStep(double TR)/g' -e 's/Objective(double TR);/Objective(TR);/g' mpc_export/acado_solver.c